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Kurzfassung 


Für die autonome Navigation mobiler Robotersysteme sind gewisse Fähig- 
keiten notwendig. Diese können in die Grundfunktionalitäten Lokalisie- 
rung, Kartierung und Bewegungsplanung unterteilt werden. Um komple- 
xere Aufgabenstellungen zu ermöglichen, können diese Funktionalitäten 
miteinander kombiniert werden. Die simultane Lokalisierung und Kar- 
tenerstellung (engl. simultaneous localization and mapping — SLAM) gilt 
dabei als eine besondere Herausforderung, da die aus den fehlerbehafteten 
Sensordaten resultierenden Unsicherheiten in der Lokalisierung und der 
erzeugten Karte voneinander abhängig sind. 

Durch die Fusion mehrerer Sensoren kann eine Reduktion der Unsi- 
cherheiten erzielt werden und folglich eine genauere Lokalisierung erreicht 
werden. Mit SLAM-Algorithmen wird durch entsprechende Berücksich- 
tigung der Abhängigkeiten zwischen der Lokalisierung und der bisher 
erstellten Karte eine konsistente und präzise Kartierung ermöglicht. 

In der vorliegenden Arbeit werden Methoden für die Kombination und 
Integration mehrerer Sensoren für die robuste und präzise Lokalisierung 
und Kartenerstellung in heterogenen Außenumgebungen vorgestellt, wel- 
che sowohl in urbanen als auch in unstrukturierten Bereichen einen mög- 
lichst robusten Einsatz erlauben sollen. 

Es wird die Fragestellung behandelt, wie zusätzliche Sensoren für 
SLAM eingesetzt werden können, um die Lokalisierung und Kartenerstel- 


lung robuster zu machen und die Präzision zu erhöhen. Eine robustere 
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Kartierung wird sowohl durch eine zuverlässigere Wiedererkennung der 
Landmarken und damit bereits besuchter Orte erfolgen als auch durch die 
Kombination von dichten und landmarkenbasierten Kartierungsverfahren. 
Weiterhin ist Teil der Untersuchungen, wie sich durch die Kombination 
von SLAM mit Multi-Sensor-Fusion zur Integration zusätzlicher absoluter 
Sensoren ein zuverlässigeres Schließen von Schleifen erzielen lässt. Außer- 
dem wird die Integration weiterer relativer Sensoren, d.h. Sensoren, deren 
Messungen sich auf die Differenz zwischen zwei Zuständen beziehen, 
betrachtet. Diese können nicht ohne Weiteres mit einfachen Filterverfah- 
ren integriert werden, weshalb in der Arbeit erweiterte Filterverfahren 
behandelt werden, um die probabilistisch korrekte Integrierbarkeit von 
zusätzlichen relativ messenden Sensoren zu ermöglichen. 

Für die zuverlässigere Wiedererkennung von Landmarken wurde ein 
erweitertes Landmarkenmodell mit zusätzlichen ortsunabhängigen und 
visuellen Merkmalen entwickelt. Durch eine probabilistische Modellierung 
kann das erweiterte Modell neben der Wiedererkennung auch in dem 
zugrundeliegenden Partikelfilter-SLAM-Algorithmus in die Berechnung 
der Partikelgewichte eingehen und damit die Robustheit weiter verbes- 
sern. 

Ebenfalls auf Basis des Partikelfilters wurde ein modularer SLAM- 
Algorithmus entwickelt, welcher über ein erweitertes Kalman-Filter pro 
Partikel die Möglichkeit bietet, konsistent weitere Sensoren oder Lokalisie- 
rungs- bzw. Kartierungsmethoden zu integrieren. Damit konnte zum einen 
ein hybrider Ansatz realisiert werden, bei dem gleichzeitig eine dichte 
2D-Rasterkarte und eine Landmarkenkarte aufgebaut werden, um deren 
gegenseitige Vorteile für eine robustere Kartierung und Lokalisierung in 
gemischten Umgebungen in ergänzender Weise zu nutzen. Zum anderen 
erlaubt der modulare SLAM-Algorithmus die Integration absoluter Sen- 
soren, welche den Vorteil mit sich bringen, dass die Unsicherheit global 
begrenzt werden kann, was das Schließen von Schleifen vereinfacht und 


letztendlich das Erstellen konsistenter Karten unterstützt. 
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Für die Integration zusätzlicher relativer Sensoren wurden Metho- 
den mit stochastischem Klonen untersucht und eine neuartige Methode 
des impliziten Klonens in Kombination mit Smoothing entwickelt. Beim 
stochastischen Klonen wird die Zustandsschätzung mit einem Klon der 
Zustandsschätzung, auf die sich eine relative Messung bezieht, und die 
Systemkovarianzmatrix erweitert. Beim impliziten Klonen kann die aug- 
mentierte Systemkovarianzmatrix bei Bedarf rekonstruiert werden und 
muss nicht durch alle Filterschritte propagiert werden. Durch das Smoo- 
thing werden Informationen von zwischenzeitlich erfassten Messungen 
anderer Sensoren auf den Klon zurückpropagiert, was die robuste Integra- 
tion mehrerer relativer und absoluter Messungen ermöglicht. 

Für die Fusion der Sensordaten zum möglichst genauen Zeitpunkt 
wurde ein Zeitstempelschätzer entwickelt, um bei der Sensoranbindung 
und -datenverarbeitung mit Rechnern ohne speziellem Echtzeitbetriebs- 
system die schwankenden Latenzen zu kompensieren. Des Weiteren wurde 
ein paralleles Verarbeitungsverfahren entwickelt, welches es erlaubt, die 
Lokalisierungsschätzung mit einer möglichst konstant hohen Rate auszu- 
geben und dennoch die Information von Sensoren zu integrieren, deren 
Verarbeitung im Vergleich zu den anderen Sensoren länger dauert und 
damit bei sequentieller Verarbeitung zu Verzögerungen führen würde. 

In einer vergleichenden Evaluation konnte gezeigt werden, dass ein 
erweitertes Landmarkenmodell mit ortsunabhängigen Merkmalen zu einer 
zuverlässigeren Datenzuordnung beitragen kann. Durch die verbesserte 
Datenzuordnung wurden bei dem SLAM-Algorithmus weniger Partikel 
für das erfolgreiche Schließen der Schleifen benötigt. Weiterhin konnte 
gezeigt werden, dass bei der Verwendung von mehreren Partikeln auch 
die Genauigkeit der Kartierung durch eine verbesserte Datenzuordnung 
gesteigert werden kann. 

Mit dem modularen SLAM-Algorithmus konnte mit einer hybriden 
Konfiguration von 2D-Rasterkarte und Landmarkenkarte gegenüber den 
Einzelverfahren eine verbesserte Kartierung und damit auch Lokalisie- 


rung erzielt werden. Auch durch die Hinzunahme eines absoluten Sensors 
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konnte die Robustheit des modularen SLAM-Algorithmus hinsichtlich des 
Schließens großer Schleifen effektiv gesteigert werden. 

Die Methode des impliziten Klonens mit Smoothing hat bei der Integra- 
tion zusätzlicher relativer Sensoren in Evaluationen mit einem simulier- 
ten GNSS-Ausfall und im Vergleich zu einer Referenzlokalisierung nahezu 
gleiche Ergebnisse erzielt wie alternative Methoden mit stochastischem 
Klonen bzw. inkrementellen Faktor-Graph-Algorithmen. Bei den Betrach- 
tungen zum Rechenaufwand hat sich gezeigt, dass die Filter-Methoden 
mit einer sehr geringen Auslastung der verfügbaren Rechenzeit für reale 
Anwendungen geeignet sind und mit dem parallelen Verarbeitungsverfah- 
ren auch rechenaufwändigere Verfahren wie Scan-Matching integrieren 
können. 

Ein weiterer Aspekt der Arbeit war die möglichst praxisnahe Umset- 
zung der entwickelten Methoden und deren Einsatz auf realen Robotersys- 
temen. So konnten mit der Implementierung der vorgestellten Algorithmen 
sowie der Umsetzung der Zeitstempelfilterung und der parallelen Verar- 
beitung umfassende Verbesserungen bei der Lokalisierung und Kartierung 
für mobile Robotersysteme geschaffen werden, welche von der Akquise 
der Sensordaten über die Fusion mehrerer relativer und absoluter Senso- 
ren bis zur Integration in SLAM-Algorithmen reichen. Ein Großteil der 
vorgestellten Algorithmen ist auf verschiedenen Forschungsplattformen 


mit unterschiedlichen Sensorausstattungen erfolgreich im Einsatz. 


Abstract 


Mobile robots require certain skills for autonomous navigation. These 
can be divided into the basic functionalities localization, mapping and 
motion planning. To enable more complex tasks, these functionalities can 
be combined with each other. Simultaneous localization and mapping 
(SLAM) is considered a particular challenge because the uncertainties in 
the localization and the generated map resulting from the noisy sensor 
data are interdependent. 

By merging several sensors, a reduction of the uncertainties and con- 
sequently a more precise localization can be achieved. With SLAM algo- 
rithms, consistent and precise mapping is made possible by taking into 
account the dependencies between the localization and the map created so 
far. 

In this thesis, methods for the combination and integration of several 
sensors for robust and precise localization and mapping in heterogeneous 
outdoor environments are presented. They are designed to allow a robust 
application in urban as well as in unstructured areas. 

The question of how additional sensors can be used for SLAM to fur- 
ther increase robustness and accuracy of the localization and mapping is 
addressed. A more robust mapping will be achieved by a more reliable 
recognition of landmarks, and thus already visited places, as well as by 
a combination of dense and landmark-based mapping methods. Further- 


more, increasing the reliability of loop closure by combining SLAM with 
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multi-sensor fusion to integrate additional absolute sensors is part ofthe in- 
vestigations. Additionally, the integration of multiple relative sensors, i.e., 
sensors whose measurements refer to the difference between two states, 
is considered. These relative measurements cannot be readily integrated 
with standard filter methods. Therefore, this thesis covers advanced filter 
methods to enable the probabilistically correct integration of additional 
relative measuring sensors. 

For a more reliable recognition of landmarks, an extended landmark 
model with additional visual and location-independent features was devel- 
oped. Through probabilistic modelling, the extended model can be used not 
only for recognition but also to calculate particle weights in the underlying 
particle filter SLAM algorithm, thus further improving robustness. 

A modular SLAM algorithm was additionally developed on the basis 
of the particle filter, which offers the possibility of consistently integrating 
further sensors, localization methods, or mapping methods via an extended 
Kalman filter per particle. On the one hand, a hybrid approach was realized, 
where a dense 2D grid map and a landmark map are built concurrently, in 
order to exploit their mutual advantages for a more robust mapping and 
localization in mixed environments in a complementary way. On the other 
hand, the modular SLAM algorithm allows the integration of absolute 
sensors, which has the advantage that the uncertainty can be bounded 
globally, simplifying the closing of loops and ultimately supporting the 
building of consistent maps. 

For the integration of additional relative sensors, methods with stochas- 
tic cloning were investigated and a novel method of implicit cloning com- 
bined with smoothing was developed. In stochastic cloning, the state is 
augmented with a clone of the state to which a relative measurement refers, 
and the system covariance matrix is also augmented. In implicit cloning, 
the augmented system covariance matrix can be reconstructed on demand 
and does not need to be propagated through all filter steps. By smoothing, 


information from intermediate measurements of other sensors is propa- 
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gated back to the clone, which enables the robust integration of several 
relative and absolute measurements. 

A time stamp estimator was developed for fusion of sensor data at as 
exact a time as possible in order to compensate for the fluctuating laten- 
cies caused by sensor data acquisition and data processing with computers 
without a special real-time operating system. Furthermore, a parallel pro- 
cessing method was developed, which allows to output the localization 
estimation at a constant high rate and still integrate the information of 
sensors, whose processing takes longer compared to the other sensors and 
therefore would lead to delays if processed sequentially. 

In a comparative evaluation, it could be shown that an extended land- 
mark model with location-independent features can contribute to a more 
reliable data association. Due to the improved data association, the SLAM 
algorithm required fewer particles to successfully close the loops. Further- 
more, it could be shown that when using multiple particles, the accuracy 
ofthe mapping can also be increased by improved data association. 

With the modular SLAM algorithm, a hybrid configuration utilizing 
a 2D grid map and a landmark map resulted in an improved mapping 
and localization compared to the individual methods. Also, by adding an 
absolute sensor, the robustness of the modular SLAM algorithm regarding 
the closing of large loops could be effectively increased. 

In evaluations with a simulated GNSS outage and in comparison to a 
reference localization, the method of implicit cloning with smoothing has 
achieved almost the same results as alternative methods with stochastic 
cloning or incremental factor graph algorithms for the integration of addi- 
tional relative sensors. Examinations of the computational demand have 
shown that the filter methods exhibit a very low utilization of the avail- 
able computing time. Thus, they are well suited for real applications and 
with the parallel processing scheme, it is possible to also integrate more 
computationally demanding methods such as scan matching. 

A further aspect of the thesis was the practical application of the de- 


veloped methods and their use on real robotic systems. Thus, with the im- 
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plementation of the presented algorithms as well as the realization of the 
time stamp filtering and parallel processing, comprehensive improvements 
in localization and mapping for mobile robot systems could be realized, 
ranging from the acquisition of sensor data, the fusion of several relative 
and absolute sensors to the integration into SLAM algorithms. A large part 
of the presented algorithms is successfully in operation on several research 
platforms with different sensor equipment. 
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Kapitel 1 


Einleitung 


Autonome Robotersysteme und künstliche Intelligenz kommen in immer 
mehr Umgebungen und in weiteren Aufgabenbereichen zum Einsatz 
[Sie04; Sic08]. Besonders die autonome Navigation mobiler Systeme hat in 
den letzten Jahren sehr viel Aufmerksamkeit erregt. 

Mit den Wettbewerben der Defense Advanced Research Projects Agency 
(DARPA), der Grand Challenge in einer Wüstenumgebung und später der 
Urban Challenge in einer städtischen Umgebung wurden die Möglich- 
keiten autonomer mobiler Systeme auch einer breiteren Öffentlichkeit 
bekannt [Thr06b; Urm08]. Teilergebnisse der bisherigen Forschungen im 
Themenfeld der autonomen Fahrzeuge sind bereits in Fahrerassistenzsys- 
teme eingeflossen [Win09]. Die Entwicklung zu komplett selbstfahrenden 
Autos im Straßenverkehr ist zwar schon weit fortgeschritten, befindet sich 
jedoch noch immer in der Weiterentwicklung [Gui11; Bad19]. 

Andere Anwendungsfelder autonomer mobiler Systeme sind abseits 
der Straße in unwegsamem Gelände oder in für Menschen gefährlichen 
oder unzugänglichen Umgebungen. Hierzu zählen unter anderem die Ent- 
wicklungen von Robotern zur Erkundung von Planeten wie einem Mars- 
Rover [Kno03; Sch14]. Weitere Forschungsfelder umfassen Rettungs- und 
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Aufklärungsroboter oder Roboter zur Überwachung von Liegenschaften 
[Cub17; Emt07]. Besonders auch in gefährlichen Umgebungen wie bei 
der zivilen Minenräumung ist der Einsatz von autonomen mobilen Sys- 
temen Gegenstand von Forschungsaktivitäten [Por14; Cur15]. Auch für 
die schnelle Aufklärung und die Durchführung von Manipulationsauf- 
gaben zur Unterstützung der Rettungsarbeiten in Katastrophenszenarien 
wird an mobilen Robotern geforscht, um möglichst nicht noch weitere 
Menschenleben zu gefährden [Kun14; Fre15; Tsa17]. Neben speziell für 
die entsprechenden Einsatzszenarien entwickelten mobilen Plattformen 
werden jedoch auch prototypisch schwere Arbeitsmaschinen wie Hydrau- 
likbagger mittels zusätzlicher Sensorik und Rechentechnik automatisiert, 
um beispielsweise kontaminierte Erdschichten abzutragen oder bei der 
Deponiesanierung zu unterstützen und damit in für den Menschen poten- 
tiell gesundheitsgefährdenden Umgebungen eingesetzt zu werden [Emt17; 
Pet19]. 

Für die autonome Navigation mobiler Robotersysteme sind gewisse 
Fähigkeiten notwendig. Diese können in die Grundfunktionalitäten Lokali- 
sierung, Kartierung und Bewegungsplanung unterteilt werden, siehe Abbil- 
dung 1.1. Die Kartierung beinhaltet dabei neben der Umgebungsmodellie- 
rung auch die Umgebungserfassung, welche auch Perzeption genannt wird. 
Die Planung kann weiter in Pfad- oder Trajektorienplanung und -regelung 
unterteilt werden [Pet13; Pet14]. Die grundlegenden Fähigkeiten sind dabei 
stark voneinander abhängig und werden für höhere Autonomiegrade mit- 
einander kombiniert [Mak02; Fai09]. So wird beispielsweise aus der Kombi- 
nation von Lokalisierung und Kartierung die simultane Lokalisierung und 
Kartenerstellung, in der englischen Literatur unter dem Akronym SLAM 
für simultaneous localization and mapping bekannt [Dur06a; Dur06b]. Für 
die Exploration hingegen ist neben der Kartierung auch noch eine Pfadpla- 
nung notwendig, wobei die aus der Umgebungserfassung erstellte Karte 
als Grundlage fiir die Pfadplanung und Hindernisvermeidung dient. 

Je nach Einsatzszenario oder Aufgabenstellung bestehen spezifische 


Unterschiede bei der Ausprägung der genannten notwendigen Fähigkei- 
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Abbildung 1.1: Fähigkeiten eines mobilen Roboters zur autonomen Navigation. Adaptiert 
von [Mak02]. 


ten. So gehört für selbstfahrende Autos, welche am Straßenverkehr teil- 
nehmen, zur Umgebungserfassung beispielsweise auch die Straßenschild- 
erkennung oder Fußgängererkennung [Wah14; Yan18]. Für das Fahren in 
unstrukturierten und unwegsamen Umgebungen spielen diese Aspekte 
hingegen keine Rolle, sondern dort ist eher die Erkennung von leichter 
Vegetation und deren Unterscheidung von harten Hindernissen wie Stei- 
nen relevant [Bra07; Ngu10]. Bei mobilen Plattformen, welche am Stra- 
ßenverkehr teilnehmen, wird vor allem in urbanen Gebieten meist auf 
präzise Karten zurückgegriffen, welche zuvor mit speziell ausgerüsteten 
Kartierungsfahrzeugen aufgenommen wurden [Zie14; Pog18]. Dagegen 
ist für die Lokalisierung in unstrukturierten Umgebungen vorher meist 
keine Karte vorhanden. Zudem sind in den erwähnten Szenarien abseits 
der Straße, z.B. zur Aufklärung in Katastrophenszenarien oder bei Erdar- 
beiten zur Dekontaminierung, Änderungen der Umgebung zu erwarten, 
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seien diese durch externe Faktoren im Katastrophenfall hervorgerufen oder 
selbst induziert bei den Aushubarbeiten bei der Dekontaminierung, sodass 
eine vorher aufgezeichnete Karte bald nicht mehr aktuell wäre. Änderun- 
gen können natürlich auch im Straßenverkehr vorkommen, hierbei handelt 
es sich jedoch meist um kleinere lokale Änderungen, wie geänderte Beschil- 
derung oder Straßenführung, welche separat mittels Änderungsdetektion 
behandelt werden können [Jo18; Pau18]. 


1.1 Problemformulierung 


In der vorliegenden Arbeit werden von den grundlegenden Fahigkeiten 
die Themen der Lokalisierung und Kartierung anhand der Sensoren des 
mobilen Robotersystems in unstrukturierten Außenumgebungen behan- 
delt, wobei die Fusion mehrerer Sensoren betrachtet wird und in der Kom- 
bination SLAM-Verfahren Verwendung finden. Die Bewegungsplanung 
und damit auch die Exploration werden in dieser Arbeit nicht behandelt. 

Die Lokalisierung, d. h. die Bestimmung der Position und Orientierung, 
bzw. Pose, des Robotersystems in seiner Umgebung auf Basis von Sensor- 
daten, kann als eine der fundamentalsten Problemstellungen betrachtet 
werden, wenn es um die Ausstattung eines mobilen Roboters mit autono- 
men Fähigkeiten geht [Cox91]. Für die Lokalisierung ist meist die Kombi- 
nation mehrerer Sensoren notwendig, da unter Umständen nicht alle Frei- 
heitsgrade von einem einzelnen Sensor gemessen werden können [Sic08]. 
Außerdem sind die Messungen aller Sensoren durch Rauschen und andere 
Störungen beeinträchtigt, sodass durch die Fusion mehrerer Sensoren die 
Robustheit und Präzision erhöht werden können. 

Eine weitere essentielle Fähigkeit ist die Erstellung konsistenter und 
präziser Karten. Diese können später für die autonome Navigation wieder- 
verwendet werden. Mit den Karten ist es für den mobilen Roboter möglich, 
sich bereits befahrene Pfade zu merken. Somit kann verhindert werden, 
dass er mehrfach in Sackgassen gerät oder lange Zeit damit verbringt, um 


Hindernisse herum zu manövrieren [Kon09]. 
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Wenn noch keine Karte der Umgebung vorhanden ist, muss diese erst 
von dem Roboter anhand seiner Sensordaten erstellt werden. Dies bein- 
haltet die bereits erwähnte simultane Kombination von Lokalisierung und 
Kartenerstellung. Da auch hier alle Messungen der Sensoren fehlerbehaf- 
tet sind, ist dies eine der größten Herausforderungen im Themenfeld der 
autonomen Navigation, denn die daraus resultierenden Fehler in der Loka- 


lisierung und in der Karte sind voneinander abhängig [Dur06a; Dur06b]. 


1.1.1 Multi-Sensor-Fusion 


Meist ist ein einzelner Sensor nicht ausreichend, um eine eindeutige Inter- 
pretation zu erlauben, da er nur begrenzte Informationen tiber die Umge- 
bung liefert [Dur88]. Auch fiir die Lokalisierung von mobilen Plattfor- 
men sind meist mehrere Sensoren notwendig, damit alle Freiheitsgrade 
geschätzt werden können. Vor allem bei der Lokalisierung im dreidimen- 
sionalen Raum, d. h. der Bestimmung von sechs Freiheitsgraden (englisch: 
degrees of freedom - 6DoF), sind mehrere Sensoren notwendig, um alle 
Freiheitsgrade schätzen zu können [Wen11]. 

Außerdem sind alle Sensoren fehlerbehaftet, d.h., deren Daten sind 
verrauscht und durch andere Störungen beeinflusst. Ein weiteres Ziel der 
Multi-Sensor-Fusion ist es daher, durch Kombination von mehreren Sen- 
soren eine Verbesserung der Genauigkeit zu erreichen, möglichst unter 
Berücksichtigung der jeweiligen Unsicherheiten, sodass unterschiedlich 
genaue Sensoren entsprechend eingebracht werden können. Dabei ist vor 
allem das Ausgleichen gegensätzlicher Nachteile bzw. sensorspezifischer 
Fehler von Bedeutung. So sind relativ messende Sensoren lokal oder kurz- 
zeitig sehr genau, können jedoch über die Zeit driften. Ein relativ mes- 
sender Sensor bedeutet dabei, dass eine Differenz zwischen dem aktuellen 
und einem vergangenen Zustand gemessen wird. Im Gegensatz dazu sind 
die Fehler von absolut messenden Sensoren zwar global begrenzt, können 
aber z.B. bei einem globalen Navigationssatellitensystem (englisch: global 


navigation satellite system — GNSS), wie dem Global Positioning System 
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(GPS, früher NAVSTAR GPS), lokal sehr ungenau sein [Bra17]. Moderne 
GNSS-Empfänger zeichnen sich dadurch aus, dass sie für die Positionsbe- 
stimmung die Satelliten mehrerer Systeme verwenden und dadurch eine 
verbesserte Zuverlässigkeit und Präzision erzielen, weil sie eine größere 
Anzahl an gleichzeitig verfügbaren Satelliten nutzen können. 

Je nach einsatzspezifischen Anforderungen oder vorhandener Sensor- 
ausstattung sind mehrere absolute und relative Sensoren zu integrieren. 
Während mehrere absolute Sensoren meist einfach zu integrieren sind, ist 
es je nach verwendeter Methode nicht ohne Weiteres möglich, mehr als 
einen relativen Sensor zu integrieren [Rou01]. 

Eine weitere Herausforderung ergibt sich bei der Integration mehrerer 
Sensoren aus der oftmals fehlenden Synchronisation der Sensoren unter- 
einander. D.h., es kann nicht davon ausgegangen werden, dass die Daten 
der unterschiedlichen Sensoren zur gleichen Zeit verfügbar sind. Außer- 
dem werden die Daten meist mit unterschiedlichen Raten von den Sensoren 
ausgegeben [Wel96]. 

Des Weiteren können einzelne Sensoren ausfallen und es ist anzustre- 
ben, dass durch die Verwendung redundanter oder aber auch heterogener 
Sensoren eine gewisse Ausfallsicherheit erreicht wird [Wen11]. 


1.1.2 Simultane Lokalisierung und Kartenerstellung 


Die Erstellung einer Karte der Umgebung mit den Sensoren eines mobilen 
Robotersystems ist eine komplexe Problemstellung, denn für die Loka- 
lisierung wird eine genaue Karte benötigt und umgekehrt wird für die 
Kartierung eine präzise Schätzung der Lokalisierung benötigt [Leo91]. So 
sind auch hier die Messungen fehlerbehaftet und zusätzlich bestehen die 
Abhängigkeiten zwischen Karte und Lokalisierung. Das bedeutet, dass die 
Unsicherheit in der Lokalisierung abhängig von den Fehlern in der bisher 
erstellten Karte ist und im Folgenden die erstellte Karte ungenau wird. Des- 
halb ist es essentiell, dass diese Abhängigkeiten in den Algorithmen berück- 


sichtigt werden, d.h. Schätzungen der Lokalisierung und die Kartierung 


1.1 Problemformulierung 


simultan berechnet werden - daher der Begriff simultane Lokalisierung 
und Kartenerstellung (englisch: simultaneous localization and mapping - 
SLAM) [Dur06a; Dur06b]. 

Bei den entstehenden Karten ist jedoch nicht nur die Prazision rele- 
vant, sondern es wird zusatzlich noch davon die Konsistenz unterschieden. 
Unter Konsistenz wird hierbei die topologisch korrekte Modellierung der 
Umgebung verstanden, d.h., dass z.B. ein bereits besuchter Ort oder Weg 
wiedererkannt wird und nicht mehrfach in der Karte auftaucht. Während 
eine geringe Präzision meist kompensierbar ist bzw. über weitere Beobach- 
tungen Verbesserungen erzielt werden können, kann sich eine fehlende 
Konsistenz eher als fatal erweisen, sofern ein Algorithmus nicht gleichzei- 
tig mehrere potentielle Kartenhypothesen berücksichtigt. Das Wiederer- 
kennen von bereits besuchten Orten ist dabei vor allem beim Schleifen- 
schluss relevant. Von Schleifenschluss wird gesprochen, wenn eine mobile 
Plattform wieder an einen bereits besuchten und kartierten Ort zurück- 
kehrt, nachdem sie in unbekanntem Gebiet unterwegs war. Hierbei können 
sich über die Zeit Fehler akkumuliert haben, die die Lokalisierungsgenau- 
igkeit stark verschlechtern und damit auch die Lokalisierung in der bereits 
aufgenommen Karte erschweren können [Thr05]. 

Bei Karten, welche auf Landmarken! basieren, ist deren robuste Wie- 
dererkennung ebenfalls eine wichtige Voraussetzung, damit eine konsis- 
tente Karte entstehen kann. Unter Wiedererkennung wird hier die Daten- 
zuordnung von Beobachtungen zu bereits kartierten Landmarken verstan- 
den. Kommt es hier zu Fehlern, können Landmarken mehrfach in der Karte 
auftauchen oder Beobachtungen zu falschen Landmarken zugeordnet wer- 
den. Dies kann zu inkonsistenten Karten führen und verschlechtert in 


jedem Fall die erzielbare Präzision [Mon07]. 


1. Eine Landmarke ist ein statisches Objekt, das sich deutlich von der Umgebung abgrenzt 
und durch Wiedererkennung zur Orientierung dient. 
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1.2 Zielsetzung der Arbeit 


Die vorliegende Arbeit beschäftigt sich mit der Erarbeitung von Metho- 
den für die Kombination und Integration mehrerer Sensoren zur präzisen 
Lokalisierung und Kartenerstellung in heterogenen Außenumgebungen. 
D.h., die entwickelten Verfahren sollen sowohl in urbanen als auch in 
unstrukturierten Bereichen möglichst robust funktionieren. 

Es wird die Fragestellung behandelt, wie zusätzliche Sensoren für 
SLAM eingesetzt werden können, um die Kartenerstellung robuster und 
präziser zu machen. Eine Steigerung der Robustheit kann durch zuverläs- 
sigeres Wiedererkennen von Landmarken und dadurch auch das Wieder- 
erkennen von bereits besuchten Orten erreicht werden, was das Erzeugen 
von konsistenten Karten vereinfachen und die Präzision der Karten stei- 
gern soll. Neben der zuverlässigeren Wiedererkennung soll die Kombina- 
tion verschiedener Kartenarten in einem hybriden Algorithmus untersucht 
werden. Auch hier ist das Ziel die Steigerung der Robustheit und der Prä- 
zision der Kartenerzeugung. 

Ein weiterer Schritt ist die Kombination von SLAM mit Multi-Sensor- 
Fusion zur Lokalisierung mit einem zusätzlichen absoluten Positionssensor. 
Durch einen ergänzenden absoluten Positionssensor kann die globale Loka- 
lisierungsunsicherheit beschränkt werden, was wiederum das Schließen 
von Schleifen vereinfacht. Es wird so bewusst auf die Beschränkung auf 
ein minimales Sensorsetup verzichtet, um den Einfluss von zusätzlichen 
Sensoren aufzuzeigen. 

Bei der Multi-Sensor-Fusion ist außerdem die Integration zusätzlicher 
relativer Sensoren, d. h. Sensoren, deren Messungen sich auf die Differenz 
zwischen zwei Zuständen beziehen, von Interesse. Diese können jedoch 
nicht ohne Weiteres mit einfachen Filtermethoden integriert werden, wes- 
halb in der Arbeit auch erweiterte Filtermethoden behandelt werden, um 
die probabilistisch korrekte Integrierbarkeit von zusätzlichen relativ mes- 
senden Sensoren zu ermöglichen. So können diese unter Berücksichtigung 
der jeweiligen Unsicherheiten zur Verbesserung der Präzision beitragen. 


1.3 Wissenschaftliche Beiträge 


Durch die Integration heterogener Sensoren kann zudem ein wechselsei- 
tiges Kompensieren von sensorspezifischen Fehlern erreicht werden. Das 
bedeutet, es können gegenseitige Nachteile ausgeglichen werden. Des Wei- 
teren soll durch die Kombination mehrerer Sensoren und entsprechende 
Algorithmen eine gewisse Ausfallsicherheit geschaffen werden. 

Die verschiedenen Sensoren sollten sich außerdem ohne zusätzliche 
aufwändige Synchronisierungshardware integrieren lassen. Vor allem bei 
prototypischen Plattformen ist dies von Vorteil, wenn neue Sensoren zu 
integrieren sind oder für Tests bzw. zu Vergleichszwecken temporär alter- 
native Sensoren anzubinden sind. 

Ein weiterer Aspekt der Arbeit ist eine möglichst praxisnahe Umset- 
zung der entwickelten Methoden und deren Evaluation mit realen Daten. 
Es soll außerdem bei allen Verfahren eine weiche Echtzeitfähigkeit gege- 
ben sein. Die weiche Echtzeitfähigkeit (englisch: soft real-time) bedeutet, 
dass vorgegebene Zeiten für die Berechnungen nicht überschritten werden 


sollten, aber gelegentlich überschritten werden dürfen [Bur09]. 


1.3 Wissenschaftliche Beiträge 


Die wissenschaftlichen Beiträge der vorliegenden Arbeit beschränken sich 
nicht auf eine einzelne Methode, sondern widmen sich der im vorange- 
gangenen Abschnitt aufgezeigten vielschichtigen Fragestellung, mehrere 
Sensoren für die Lokalisierung und Kartenerstellung einzusetzen und zu 
kombinieren. Aus der Vielschichtigkeit leitet sich die Aufteilung in unter- 
schiedliche Themenblöcke ab: 


- Ein erweitertes SLAM-Verfahren wurde entwickelt, um die Wie- 
dererkennung von Landmarken robuster zu machen. Hierfür wurde 
ein erweitertes Landmarkenmodell entworfen, welches neben der 
Position weitere positionsunabhängige Merkmale einbezieht und 
somit die Wiedererkennung von der Lokalisierungsunsicherheit 


weniger abhängig macht [Emt10c]. 
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e Mit der Kombination verschiedener Kartenarten wurde ein 
hybrides SLAM-Verfahren entwickelt, welches die Genauigkeit der 
Kartierung verbessert [Emt12b]. 


e Durch die Kombination von Multi-Sensor-Fusion mit SLAM, 
indem zusätzlich ein absoluter Positionssensor mit einbezogen wird, 
konnte gezeigt werden, dass durch die Beschränkung der globalen 
Lokalisierungsunsicherheit das Schließen von Schleifen vereinfacht 
wird [Emt12a]. 


e Mit einer Methode auf Basis des stochastischen Klonens in Kombina- 
tion mit Smoothing wurde ein neuartiges Filterverfahren zur Fusion 
von mehreren relativen und absoluten Sensoren für die Lokali- 
sierung und Kartierung in sechs Freiheitsgraden entwickelt [Emt18b; 
Emt19b; Emt19a]. 


Für eine Abrundung der Arbeit hinsichtlich der Praxistauglichkeit 
wurden zusätzlich noch ein Verfahren zur Synchronisation über 
Zeitstempel und ein Verfahren zur Parallelverarbeitung für die 
Integration verzögerter Sensordaten entwickelt [Kle20; Emt14; 
Emt18b]. 


Sämtliche vorgestellten neuen Methoden und Verfahren wurden anhand 
von realen Sensordaten evaluiert und wo möglich mit etablierten wissen- 


schaftlichen Verfahren in Vergleich gesetzt. 


1.4 Aufbau der Arbeit 


Das folgende Kapitel 2 gibt einen Überblick über den Stand der Forschung. 
Hierbei liegt das Augenmerk auf der Multi-Sensor-Fusion für die Lokali- 
sierung mobiler Robotersysteme und der simultanen Lokalisierung und 
Kartenerstellung. Anschließend werden in Kapitel 3 die theoretischen 
Grundlagen zu Multi-Sensor-Fusion und SLAM behandelt, wobei sich die 


vorgestellten Themen auf die für die Arbeit relevanten Methoden und 
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Verfahren beschränken. In den Kapiteln 4 und 5 werden die in dieser 
Arbeit entstandenen Methoden und Verfahren vorgestellt. Den Anfang 
machen hier die erweiterten Methoden zur simultanen Lokalisierung und 
Kartierung in Abschnitt 4.1 mit dem erweiterten Landmarkenmodell und 
in Abschnitt 4.3 mit der Kombination verschiedener Kartenarten. Es folgt 
in Abschnitt 5.1 die Kombination von Multi-Sensor-Fusion mit SLAM zur 
Verbesserung des Schleifenschlusses mittels zusätzlichem absoluten Sen- 
sor. Die Fusion von mehreren relativen und absoluten Sensoren wird in 
Abschnitt 5.2 vorgestellt, gefolgt von dem Verfahren zur Synchronisation 
und dem Verfahren zur Parallelverarbeitung in Abschnitt 5.3.4. Anschlie- 
ßend werden in Kapitel 6 die jeweiligen Ergebnisse zu allen neu entwi- 
ckelten Methoden anhand von Evaluationen mit realen Sensordaten und 
im Vergleich zu bestehenden Methoden präsentiert. Abschließend wird 
die Arbeit in Kapitel 7 zusammengefasst und ein Ausblick auf mögliche 
weiterführende Arbeiten gegeben. 


Kapitel 2 


Stand der Forschung 


In diesem Kapitel wird der Stand der Forschung der Multi-Sensor-Fusion 
im Kontext der Lokalisierung und der simultanen Lokalisierung und Kar- 


tenerstellung für autonome mobile Robotersysteme aufgeführt. 


2.1 Multi-Sensor-Fusion für die Lokalisierung 


Für die Fusion mehrerer Sensoren zur Lokalisierung in der Navigation auto- 
nomer mobiler Systeme sind verschiedene Varianten des Kalman-Filters 
die am weitesten verbreiteten Methoden. Sie gehören zu der Klasse der 
rekursiven bayesschen Schätzer und es gehen daher auch die Messunsi- 
cherheiten der Sensoren in die Schätzung mit ein. Daneben gibt es auch 
andere Filterverfahren wie Partikelfilter für nicht-parametrische Dichtere- 
präsentationen oder auf graphischen Modellen basierende Verfahren wie 
Faktor-Graphen, jedoch sind Kalman-Filter aufgrund ihrer Leistungsfähig- 
keit und Robustheit in den meisten Anwendungen zur Lokalisierung in 


der Navigation vorherrschend. 


2 Stand der Forschung 


2.1.1 Kalman-Filter 


Das Kalman-Filter ist ein Optimalfilter für lineare Systeme und ist in unter- 
schiedlichen Varianten mit Erweiterungen eines der meistverwendeten 
Verfahren für die Fusion mehrerer Sensoren. Das Filter wurde als Least- 
Squares-Optimalschätzer für lineare Systeme von Rudolf E. Kalman 1960 
vorgestellt [Kal60]. Die Einordnung in die bayessche Statistik als rekursi- 
ves Bayes-Filter erfolgte erst später [Ho64]. Das Filter ist am bekanntesten 
als rekursiver bayesscher Schätzer für lineare Systeme mit gaußverteil- 
tem Systemmodell, es ist aber auch ein optimaler linearer Schätzer, wenn 
das Rauschen nicht gaußverteilt ist [Sim06]. Die rekursive Filterung ist 
in zwei Phasen aufgeteilt. Die erste Phase ist die Prädiktion, bei der der 
Zustand vom vorherigen Filterschritt mittels eines Systemmodells, wel- 
ches auch Messungen von relativen Sensoren beinhalten kann, zu einer 
A-priori-Schätzung fortgeführt wird. Die zweite Phase ist der Korrektur- 
schritt, welcher eine Messung mit der A-priori-Schätzung kombiniert und 
eine verbesserte A-posteriori-Schätzung liefert [Wel95; Goe99]. 

Bereits sehr kurze Zeit nach der Vorstellung des Kalman-Filters erfolgte 
1961 die Erweiterung für nichtlineare Systeme in Form des erweiterten 
Kalman-Filters (englisch: extended Kalman Filter - EKF) für die Apollo- 
Missionen der National Aeronautics and Space Administration (NASA) 
[McG85]. Das EKF ermöglicht die Anwendung auf nichtlineare Systeme 
durch Linearisierung um die aktuelle Schätzung Da die nichtlinearen Funk- 
tionen nicht auf die Matrizen angewendet werden können, werden die 
Matrizen der ersten partiellen Ableitungen - die Jacobi-Matrizen — mit 
der aktuellen Schätzung ausgewertet und in die linearen Kalman-Filter- 
Gleichungen eingesetzt. In der Anwendung für die Lokalisierung autono- 
mer mobiler Roboter werden hauptsächlich erweiterte Kalman-Filter ein- 
gesetzt, da hier nichtlineare System- und Messmodelle vorliegen [Mor08; 
Gre10; Agh13]. 

Ein weiteres Anwendungsgebiet bei der Lokalisierung ist die INS/GNSS- 
Integration. Dabei werden ein inertiales Navigationssystem (englisch: 
inertial navigation system — INS), welches Beschleunigungsmesser und 
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Drehratensensoren kombiniert, und ein GNSS fusioniert. Durch die Fusion 
können die lokalen Abweichungen des GNSS durch das INS und umge- 
kehrt die systematische Messabweichung der INS-Sensoren durch das 
GNSS kompensiert werden. Auch bei der INS/GNSS-Integration kommen 
meistens erweiterte Kalman-Filter zum Einsatz [Moh01; Wen11; Gro08; 
Far08]. 

Eine wichtige Eigenschaft des Kalman-Filters ist die Möglichkeit der 
Fusion von mehreren asynchronen Sensoren in Form von individuelle Ein- 
zelmessungen, welche auch nur partielle Information für die Korrektu- 
ren liefern können. Dies wurde in den Arbeiten von Welch et al. anhand 
einer auf einem EKF basierenden Methode gezeigt [Wel96; Wel97]. Die 
Methode wurde SCAAT genannt, was für single-constraint-at-a-time steht 
und bedeutet, dass Einzelmessungen als individuelle Korrekturschritte inte- 
griert werden können. Dies erlaubt neben der einfacheren Integrierbarkeit 
mehrerer Sensoren eine hohe Ausgaberate der Schätzung im Vergleich zu 
anderen Systemen, da nicht mehrere Messungen zusammengefasst wer- 
den müssen, sondern einzeln als individuelle Korrekturschritte eingespeist 
werden können. 

Die Sigma-point-Kalman-Filter sind eine Alternative zum erweiter- 
ten Kalman-Filter für stark nichtlineare Systeme [Mer04]. Sie zeichnen 
sich durch deterministisches Sampling der Gaußdichte aus und während 
das EKF nur die ersten zwei statistischen Momente, d.h. Mittelwert und 
Varianz, modelliert, können mit den Sigma-point-Kalman-Filtern auch 
höhere Momente erfasst werden. Das bekannteste Filter dieser Art ist 
das Unscented-Kalman-Filter (UKF) [Jul95; Jul97]. Während beim EKF 
die nichtlinearen Funktionen linearisiert werden, werden beim UKF die 
durch deterministisches Sampling gezogenen Punkte durch die nichtlinea- 
ren Funktionen transformiert und anschließend aus den transformierten 
Samples die Dichte rekonstruiert [Jul04b; Jul04a]. Beim UKF können so 
die ersten drei statistischen Momente abgebildet werden. Somit können 
Verbesserungen für stark nichtlineare Systeme bei vergleichbarem Rechen- 


aufwand erzielt werden. Ein weiterer Vorteil der nichtlinearen Transfor- 
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mation der Punkte ist, dass keine Jacobi-Matrizen hergeleitet werden 
müssen. Nachteilig sind die zusätzlichen freien Parameter für die Wahl des 
Samplings, welche zusätzlich bestimmt werden müssen. 

Mit den Sigma-point-Kalman-Filtern können Nichtlinearitäten zwar 
sehr viel besser erfasst werden als mit einem EKF, sie werden jedoch für die 
Lokalisierung eher selten angewendet, da sie hier kaum Vorteile bringen. 
So haben sich für die Orientierungsschätzung mittels Quaternionen keine 
Vorteile des UKF gegenüber einem EKF gezeigt [LaV03]. Auch für die 2D- 
Lokalisierung eines mobilen Roboters mittels Ultraschallsensoren hatte 
sich gezeigt, dass ein EKF genauso gute Ergebnisse wie ein UKF liefern 
kann, d.h., dass auch für diesen Fall die Nichtlinearitäten im Modell nicht 
so groß sind, als dass ein UKF von Vorteil wäre [DAl15]. Für die INS/GNSS- 
Integration konnte ebenfalls gezeigt werden, dass ein Sigma-point-Kalman- 
Filter keine Vorteile hinsichtlich der Präzision oder Robustheit gegenüber 
einem EKF bietet [Wen11]. 


2.1.2 Stochastisches Klonen 


Während mehrere absolute Sensoren einfach in die in den vorangegan- 
genen Abschnitten vorgestellten Kalman-Filter-Varianten zu integrieren 
sind, ist die Fusion von mehreren relativen Sensoren mit ihnen nicht for- 
mal korrekt möglich, da die Filter auf der Markov-Annahme basieren. Die 
Markov-Annahme besagt, dass der aktuelle Zustand gegeben dem vorhe- 
rigen Zustand unabhängig von allen weiter zurückliegenden Zuständen 
ist und dass die aktuelle Messung gegeben dem aktuellen Zustand bedingt 
unabhängig von allen vorherigen Messungen und Zuständen ist [Sar13]. 
Eine relative Messung hängt jedoch von einem vorherigen Zustand ab und 
verletzt damit die Markov-Annahme. 

Ein einzelner relativer Sensor kann für die Prädiktion verwendet und 
mit beliebig vielen absoluten Sensoren mittels der genannten Kalman- 
Filter-Varianten fusioniert werden. Kommt jedoch ein zweiter relativer 


Sensor hinzu, kann dieser nicht ohne Weiteres als Korrekturschritt inte- 
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griert werden. Weil eine relative Messung Differenzen zwischen einem 
vorangegangenen und dem aktuellen Zustand misst, werden hierdurch 
Korrelationen eingeführt, welche die Markov-Annahme verletzen. Metho- 
den zur Rekonstruktion können zwar angewendet werden, sie sind jedoch 
mit Nachteilen verbunden. Eine Methode ist die Berechnung einer pseudo- 
absoluten Messung aus dem letzten Zustand und der relativen Messung 
[Hof99]. Diese ist jedoch suboptimal, da hierdurch das Filter überoptimis- 
tisch wird und folglich andere Sensoren zu wenig eingehen. Alternativ 
kann durch Ableitung aus der relativen Messung eine absolute Messung 
rekonstruiert werden, wie z.B. durch Bildung des Differenzenquotienten 
aus einer Strecke und der Zeit die Durchschnittsgeschwindigkeit ableit- 
bar ist. Hierzu müssen die Messungen jedoch in einer hohen Rate zur 
Verfügung stehen und auch im Zustandsraum abgebildet werden [Rou02]. 
Zusätzlich kommt es bei der Ableitung von verrauschten Signalen zu einem 
Anstieg des hochfrequenten Rauschens [Pic16]. Eine weitere Möglich- 
keit besteht darin, die relativen Sensoren alternierend einzusetzen, wobei 
bevorzugt der genaueste Sensor verwendet wird und der alternative Sen- 
sor nur dann, wenn der Hauptsensor aussetzt [Agr06]. Hierdurch werden 
zwar keine Approximationen notwendig, es werden jedoch nicht alle vor- 
handenen Informationen einbezogen, was ebenfalls sub-optimal ist. 

Zur formal korrekten Integration von zusätzlichen relativen Messun- 
gen stellten Roumeliotis et al. stochastic cloning mit einem Kalman-Filter 
vor [Rou01; Rou02]. Hierbei wird der Zustand mit einem Klon des zurück- 
liegenden Zustands erweitert, womit sich dessen Korrelationen mit dem 
aktuellen Zustand korrekt abbilden lassen. Das Verfahren wurde daher 
Stochastic-cloning-Kalman-Filter (SC-KF) genannt. In den genannten 
Arbeiten wurden zwei relative Sensoren fusioniert und es konnte gezeigt 
werden, dass in diesem Fall nicht explizit geklont werden muss, sondern 
eine Rekonstruktion der erweiterten Systemkovarianzmatrix erfolgen 
kann. Auch in späteren Arbeiten wurden zwei relative Sensoren fusioniert, 


aber es wurde darauf hingewiesen, dass diese vereinfachende Rekonstruk- 
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tion bei der Hinzunahme weiterer Sensoren nicht mehr gültig ist und ein 
explizites Klonen erforderlich wird [Mou06; Mou07]. 

In [Chi11] wurden ebenfalls nur relative Sensoren auf einem Laufrobo- 
ter miteinander fusioniert. Neben einer IMU wurden visuelle Odometrie 
und die Odometrie der Beine integriert. In dieser Arbeit wurde ein indirek- 
tes Informationsfilter statt eines Kalman-Filters eingesetzt. Beim Informa- 
tionsfilter handelt es sich um die inverse Formulierung des Kalman-Filters, 
d.h., es werden statt dem Zustand und der Kovarianzmatrix die Informa- 
tion und die Informationsmatrix zugrunde gelegt [Gro02]. In der Arbeit 
wird nicht der komplette geschätzte Zustand geklont, sondern nur rele- 
vante Zustandsvariablen, um Rechenzeit zu sparen. Als relevante Zustands- 
variablen wurden die der Pose identifiziert, da die verwendeten relativen 
Sensoren die Differenz zwischen zwei Posen messen. In [Kle11] wurde 
für die Personennavigation mit einem EKF mit stochastischem Klonen 
zusätzlich ein GPS als absoluter Sensor integriert, wobei ebenfalls nicht 
der komplette geschätzte Zustand geklont wurde, sondern nur die Position. 

In [Lyn13] wird ebenfalls die Fusion mit absoluten Sensoren integriert. 
Das Filter ist so aufgebaut, dass wie in den ursprünglichen Veröffentlichun- 
gen von Roumeliotis et al. kein explizites Klonen erfolgt, obwohl dies bei 
der Hinzunahme absoluter Sensoren erfolgen müsste, siehe [Mou07]. Für 
die zusätzlichen relativen Sensoren werden jedoch in den Zustand weitere 


Variablen aufgenommen, welche die Drift der relativen Sensoren abbilden. 


2.1.3 Partikelfilter 


Das Partikelfilter ist ebenfalls ein rekursives Bayes-Filter. Es ist jedoch 
kein parametrisches Filter wie das Kalman-Filter, sondern modelliert die 
Wahrscheinlichkeit durch Sampling des Zustandsraums und kann dadurch 
prinzipiell beliebige Dichten repräsentieren. Als rekursives Bayes-Filter 
existieren bei einem Partikelfilter ebenfalls zwei Phasen wie beim Kalman- 
Filter. In der ersten Phase wird für jedes Partikel basierend auf der Sys- 


2.1 Multi-Sensor-Fusion für die Lokalisierung 


temdynamik ein Sample des neuen Zustands gezogen und in der zweiten 
Phase mit den Beobachtungen gewichtet [Che03]. 

Im Kontext der Navigation mobiler Robotersysteme werden Partikel- 
filter meist für SLAM eingesetzt, siehe Abschnitt 2.2. Bei der reinen Loka- 
lisierung können Partikelfilter jedoch auch von Vorteil sein, wenn schon 
eine Karte vorhanden ist, vor allem wenn Mehrdeutigkeiten zu erwarten 
sind, da auch multimodale Dichten abgebildet werden können. In [Del99] 
wurde ein Partikelfilter für die 2D-Lokalisierung von mobilen Robotern in 
bekannten Karten mittels Sonar oder 2D-LiDAR (englisch: light detection 
and ranging - LiDAR) vorgestellt. Da mit dem Partikelfilter eine multimo- 
dale Dichte modelliert wird, konnte der Roboter aus einer völlig unbekann- 
ten Anfangspose über die Zeit auch in mehrdeutigen Umgebungen seine 
Pose eindeutig bestimmen. Für die Lokalisierung in einer bereits vorhan- 
denen Karte existieren noch weitere auf Bayes-Filtern basierende Metho- 
den wie topologische Ansätze oder auf Rasterisierung basierende Ansätze, 
diese haben jedoch signifikante Nachteile bezüglich der Genauigkeit bzw. 
Effizienz [Fox03]. 

Zur Lokalisierung mobiler Roboter mit Partikelfiltern ohne bekannte 
Karte gibt es sehr wenige Arbeiten und Veröffentlichungen. Zwar konnte 
für die 2D-Lokalisierung eines mobilen Roboters in Simulationen eine bes- 
sere Schätzung im Vergleich zum erweiterten Kalman-Filter erzielt werden, 
allerdings mit einem vielfachen Rechenaufwand [Rig07]. Bei der Schätzung 
einer Lokalisierung in 3D ergibt sich im Vergleich zur Schätzung in 2D ein 
sehr viel höherdimensionaler Zustandsraum, da neben den sechs Freiheits- 
graden zusätzlich die systematischen Abweichungen der Sensoren einer 
inertialen Messeinheit (englisch: inertial measurement unit — IMU) hin- 
zukommen und dadurch sehr viele Partikel notwendig werden, um den 
Zustandsraum noch adäquat abzudecken. In einer Untersuchung konnte 
jedoch durch eine Marginalisierung, bei der der lineare Teil mit einem 
Kalman-Filter und nur der nichtlineare Teil mit einem Partikelfilter berech- 


net wurde, die notwendige Partikelzahl signifikant verringert werden. Den- 
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noch hat sich ein zum Vergleich herangezogener EKF für die Posenschät- 


zung als präziser und robuster erwiesen [Ble08]. 


2.1.4 Faktor-Graphen 


Bei Faktor-Graphen handelt es sich um ein graphisches Modell, welches zur 
Schätzung der A-posteriori-Wahrscheinlichkeit über die Zeit dient [Ind12]. 
Ein Faktor-Graph ist ein bipartiter Graph und besteht aus Variablenknoten, 
welche den zu schätzenden Systemzuständen entsprechen, und Faktorkno- 
ten, welche die Messungen oder A-priori-Informationen repräsentieren. 
Da es sich um einen bipartiten Graphen handelt, verlaufen die Kanten nur 
zwischen Variablenknoten und Faktorknoten. Bei der Lokalisierung sind 
jeweils die einzelnen Posen Variablenknoten und die Navigationslösung 
des gesamten Pfades wird dann über eine nichtlineare Optimierung des 
Graphen geschätzt [Del12]. 

Da der Rechenaufwand zur Berechnung des gesamten Graphen mit 
dessen Größe anwächst, sind diese zunächst nicht für Anwendungen mit 
Echtzeitanforderungen geeignet. Der Anstieg des Rechenaufwands hängt 
hierbei stark von der Art der Problemstellung ab, da diese die Struktur des 
Graphen beeinflusst. Bei der Lokalisierung sind die Knoten des Graphen 
meist nur mit jeweils wenigen anderen Knoten verknüpft, sodass mittels 
Ansätzen für dünnbesetzte lineare Berechnungen (englisch: sparse linear 
algebra) eine effizientere Berechnung realisiert werden kann [Gri10; Del12]. 
In [Ind13] wurden zusätzlich zwei entscheidende Erweiterungen angewen- 
det, um eine echtzeitfähige Lokalisierung in 3D mittels IMU und visueller 
Odometrie (auch bekannt als visuelle inertiale Navigation) zu realisieren. 
Die erste Erweiterung ist ein IMU-Faktor, welcher mehrere aufeinander- 
folgende IMU-Messungen bündelt, damit die Knotenanzahl kleiner gehal- 
ten werden kann. Die zweite Erweiterung basiert auf einem Verfahren - 
iSAM2 -, mit dem der Graph nicht in jedem Schritt komplett neu optimiert 
werden muss, sondern inkrementell auf vorherige Ergebnisse aufbauen 


kann [Kae12]. Mit diesen Erweiterungen konnten zwar geringe Berech- 
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nungszeiten erreicht werden, es traten aber einige Ausreißer auf, welche 
über den Echtzeitanforderungen lagen. In einer späteren Veröffentlichung 
mit einem verbesserten IMU-Faktor und mit zusätzlich besser optimiertem 
iSAM2 konnte dann die prinzipielle Echtzeitfähigkeit für visuelle inertiale 


Navigation gezeigt werden [For15]. 


2.2 Simultane Lokalisierung und 
Kartenerstellung 


Bei der simultanen Lokalisierung und Kartenerstellung sind verschiedene 
Aspekte relevant. So gibt es unterschiedliche SLAM-Methoden für die 
Lösung des Problems. Des Weiteren können, je nach eingesetztem perzep- 
torischem Sensor, verschiedene Kartenarten unterschieden werden. Außer- 
dem ist die Wiedererkennung von bereits kartierten Landmarken oder 
Orten von großer Bedeutung. Die genannten Aspekte lassen sich je nach 
Ansatz nicht immer scharf trennen, da teilweise die Kartenart eng mit der 
SLAM-Methode oder der Art des Wiedererkennungsverfahrens verknüpft 


ist. 


2.2.1 Methoden 


Da alle Sensordaten fehlerbehaftet sind und die Abhängigkeiten der Loka- 
lisierung und der Karte zu modellieren sind, kommen bei SLAM meist pro- 
babilistische Methoden zum Einsatz. Ein konzeptueller Durchbruch wurde 
mit der Erkenntnis erzielt, dass das SLAM-Problem konvergiert, wenn die 
Lokalisierung und die Karte als ein gemeinsames Schätzproblem formu- 
liert werden, d.h., dass die volle A-posteriori-Wahrscheinlichkeit über die 


Lokalisierung und die Karte berechnet wird [Dur06a]. 
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Bayes-Filter 


Ein großer Anteil der SLAM-Verfahren basiert aufrekursiven Bayes-Filtern, 
was erweiterte Kalman-Filter-Varianten und Partikelfilter umfasst [Dur06a; 
Aul08]. 

Mit der Annahme eines gaußverteilten Bewegungs- und Messmodells 
sind auch bei SLAM erweiterte Kalman-Filter im Einsatz. Die A-posteriori- 
Wahrscheinlichkeit des SLAM-Problems ist dann beim EKF-SLAM eben- 
falls eine multivariate Gaußverteilung [Dis01]. 

Da die volle A-posteriori-Wahrscheinlichkeit berechnet wird, werden 
bei EKF-SLAM alle Landmarken Teil des Zustands. D. h., der Speicherbe- 
darf wächst unter Vernachlässigung der Pose des Roboters quadratisch 
mit der Anzahl der Landmarken N und die Laufzeit liegt bei mindestens 
O(N?>77) aufgrund der Matrixmultiplikationen!. 

Um dem bei vielen Landmarken anwachsenden Rechenaufwand des 
EKF-SLAM entgegenzuwirken, wurden verschiedene Verfahren entwickelt. 
Eines davon reduziert die Anzahl der Landmarken, indem relevante Land- 
marken selektiert und unwichtige entfernt werden [Dis00]. Durch das Ent- 
fernen von Landmarken wird jedoch die Lokalisierungsgenauigkeit ver- 
ringert, sodass hier ein Kompromiss gefunden werden muss. Mit einem 
anderen Verfahren, dem komprimierten EKF (englisch: compressed EKF - 
CEKF), werden nur lokale SLAM-Aktualisierungen mit der vollen Rate 
durchgeführt und später auf die volle SLAM-Lösung propagiert [Gui01; 
Gui02b]. Dadurch ist der Rechenaufwand meistens niedrig, während nur 
Aktualisierungen für die Landmarken in der lokalen Umgebung berech- 
net werden. Erst wenn das lokale Gebiet verlassen wird, muss eine volle 
Aktualisierung durchgeführt werden. Diese volle Aktualisierung ist ohne 
Informationsverlust möglich, sodass die Lokalisierungsgenauigkeit nicht 
verringert wird. Eine weitere Methode basiert auf der inversen Formu- 


lierung des EKF, dem erweiterten Informationsfilter. Sie ist eine approxi- 


1. Die Laufzeit der Matrizenmultiplikation ist © (N°) mit dem naiven Standardalgorithmus. 
Mit optimierten Algorithmen liegt die Laufzeit niedriger und befindet sich momentan 
bestenfalls bei © (N ??737) [Wil12]. 
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mative Methode, welche auf der Erkenntnis beruht, dass die meisten Ele- 
mente der Informationsmatrix außerhalb der Diagonalen nahe 0 sind und 
mit entsprechender Verarbeitung effiziente Aktualisierungsalgorithmen 
für dünnbesetzte Matrizen eingesetzt werden können [Thr04; Eus05]. 
Neben dem hohen Rechenaufwand ist bei EKF-SLAM nachteilig, dass 
nur unimodale Dichten modelliert werden können und nur eine Hypothese 
für die Datenzuordnungen der Beobachtungen zu den kartierten Landmar- 
ken verfolgt werden kann, was den Algorithmus anfällig für mehrdeutige 
Situationen macht. Diese können gravierende Folgen haben, da die Integra- 
tion falscher Beobachtungen nicht mehr rückgängig gemacht werden kann. 
Treten zu viele Fehlzuordnungen auf, kann der Algorithmus divergieren. 
Um den Einfluss falscher Zuordnungen zu verringern, wurde im Kontext 
der Zielverfolgung ein Verfahren entwickelt, welches mehrere Hypothesen 
verfolgen kann [Rei79]. Da für jede Hypothese ein eigenes Filter berechnet 
werden muss, vervielfacht sich jedoch der Rechenaufwand, weshalb dieser 
Ansatz noch nicht mit EKF-SLAM kombiniert wurde [Dur06b]. 
Partikelfilter können hingegen implizit mehrere Hypothesen bezüglich 
der Datenzuordnung verfolgen und auch multi-modale Dichten modellie- 
ren. Partikelfilter repräsentieren eine Wahrscheinlichkeitsdichte mit einem 
Set von gewichteten Samples, welche entsprechend der Wahrscheinlich- 
keit des Zustands verteilt und gewichtet sind. Sie sind allerdings nur für 
niedrig-dimensionale Probleme geeignet, da die Anzahl der benötigten 
Partikel mit der Dimension des Zustandsraums ansteigt. Da die Dimen- 
sion des SLAM-Problems mit der Anzahl der Landmarken ansteigt, ist 
die direkte Anwendung von Partikelfiltern nicht möglich. Es ist jedoch 
möglich, die benötigte Anzahl von Partikeln durch die Anwendung einer 
Rao-Blackwellisierung zu reduzieren. Bei dem daraus entstehenden rao- 
blackwellisierten Partikelfilter (englisch: rao-blackwellized particle filter - 
RBPF) wird nicht mehr nur die Pose des Roboters geschätzt, sondern des- 
sen Pfad, wodurch die Landmarken bedingt unabhängig werden und das 
SLAM-Problem in einzelne, unabhängige Landmarkenschätzer faktorisiert 
werden kann [Mur99; Dou00a]. Mit dem FastSLAM genannten Algorith- 
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mus wurde eine effiziente Umsetzung eines RBPF vorgestellt, bei dem der 
Pfad des Roboters durch das Partikelfilter geschätzt wird und jedes Par- 
tikel eine eigene Karte mit jeweils unabhängigen Landmarkenschätzern 
mit sich führt [Mon02]. Ein mögliches Problem bei dem RBPF ist, dass die 
Partikel durch die Systemdynamik in der ersten Phase möglicherweise zu 
weit verteilt werden und viele Partikel in der zweiten Phase sehr geringe 
Gewichte erhalten werden. Mit FastSLAM 2.0 wurde dies behandelt, indem 
die Beobachtungen bereits in die erste Phase mit eingehen, wodurch die 
Partikel näher an der A-posteriori-Wahrscheinlichkeit liegen und damit 
auch höhere Gewichte erhalten [Mon03b]. Es konnte gezeigt werden, dass 
durch diese Verbesserung im Vergleich zum originalen FastSLAM wesent- 
lich weniger Partikel notwendig sind, und es konnte sogar ein Beweis 
geführt werden, dass der Algorithmus mit nur einem Partikel konvergiert. 

Wenn die zwei Phasen des Partikelfilters für lange Zeit fortgesetzt wer- 
den, kann es zu einer sogenannten Degeneration kommen, was bedeutet, 
dass viele Partikel ein sehr geringes Gewicht bekommen. Da diese Parti- 
kel einen Zustand mit sehr geringer Wahrscheinlichkeit repräsentieren, 
ist es sinnvoll, diese auszusortieren. Für ein statistisch korrektes Aussor- 
tieren wurde die Methode des sequential importance resampling (SIR) von 
Rubin entwickelt [Rub88]. Bei dieser Methode wird vor jeder ersten Phase 
ein neues Set aus ungewichteten Partikeln aus dem aktuellen Set gezogen 
mit Zurücklegen mit einer Wahrscheinlichkeit proportional zum jeweili- 
gen Gewicht. Diese Methode findet auch in den erwähnten FastSLAM und 
FastSLAM 2.0 SLAM-Algorithmen Verwendung. 

Durch das Ziehen mit Zurücklegen werden Partikel mit hohen Gewich- 
ten mehrfach gezogen und Partikel mit geringen Gewichten werden dafür 
aussortiert. Wenn das Resampling in jedem Schritt ausgeführt wird, kann 
u.U. über die Zeit die Diversität zu sehr verringert werden. D.h., die 
adäquate Abdeckung des Zustandsraums ist dann nicht mehr gegeben. 
Um diesen Effekt abzuschwächen, wurde ein adaptives Resampling einge- 
führt, was nur dann durchgeführt wird, wenn die Gewichte der Partikel 


zu stark variieren [Dou00b]. 
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Für die 2D-Kartierung haben sich die Partikelfilter vorteilhaft gegen- 
über den Kalman-Filtern gezeigt. Für SLAM mit 3D-Sensoren sind sie 
jedoch weniger geeignet. Zum einen werden sehr viele Partikel für die 
Abdeckung des größeren Zustandsraums benötigt und zum anderen wird 
eine Karte pro Partikel benötigt, was zu unpraktikablen Speicheranforde- 


rungen führen würde. 


Graph-basierte Verfahren 


Das SLAM-Problem lässt sich auch als Graph formulieren. Die Elemente 
des Graphen sind die Posen des Roboters über die Zeit und Messungen 
werden als Randbedingungen zwischen den Posen modelliert. Die beste 
Lösung entspricht der Konstellation aller Posen, welche mit allen Messun- 
gen am konsistentesten ist. Da den Posen auch Messungen der Umgebung 
zugeordnet sind, wird so auch implizit eine Karte mitgeschätzt. Für das 
Finden der Lösung wird meist auf Methoden der kleinsten Quadrate (eng- 
lisch: least squares) zur Fehlerminimierung zurückgegriffen und da der 
volle Pfad geschätzt wird, werden diese Ansätze auch oft als Smoothing 
bezeichnet [Gri10]. 

In [Lu97] wurde das SLAM-Problem als ein Netzwerk aus räumli- 
chen Relationen durch Rad-Odometrie und das Scan-Matching mit 2D- 
Laserscans formuliert. Der Pfad wird anschließend mittels eines iterativen 
Optimierungsalgorithmus geschätzt. Die initiale Schätzung der Posen für 
den iterativen Algorithmus wird aus der Odometrie berechnet. Wenn diese 
zu ungenau ist, kann es vorkommen, dass der Algorithmus in ein lokales 
Minimum konvergiert, was zu einer inkonsistenten Karte führen kann. Um 
eine Erkennung von Schleifen zu ermöglichen, wurde in darauf aufbauen- 
den Arbeiten neben anderen Verbesserungen ein zusätzliches Verfahren 
eingeführt, um schon besuchte Orte durch Korrelation von Kartenteilen 
wiederzuerkennen [Gut99; Gut00]. 

Da der Rechenaufwand mit der Größe des Graphen stark ansteigt, gibt 
es Methoden für die Reduktion der Komplexität des Gesamtproblems. Mit 
dem Atlas genannten Ansatz wurde hierfür eine Graphstruktur mit lokalen 
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Karten vorgestellt. Dabei wurde innerhalb der lokalen Karten mit einem 
erweiterten Kalman-Filter kartiert und nur die globalen Zusammenhänge 
zwischen den lokalen Karten mit einem Graphen modelliert, wodurch die- 
ser mit weniger Elementen effizienter optimiert werden konnte [Bos03; 
Bos04]. 

In [Kae12] wurde das schon in Abschnitt 2.1.4 erwähnte Verfahren 
iSAM2 für SLAM vorgestellt. Es erlaubt eine sehr effiziente inkrementelle 
Optimierung des Faktor-Graphen und ist damit prinzipiell für Echtzeit- 
anwendungen geeignet. So ist bei der Befahrung und Kartierung neuer 
Gebiete nur die Neuberechnung eines kleinen Teils des Graphen notwen- 
dig. Bei dem Schließen von Schleifen müssen jedoch große Teile des Gra- 
phen neu optimiert werden, sodass immer wieder längere Berechnungszei- 
ten auftreten können und damit die Echtzeitfähigkeit beeinträchtigt wird. 
Um eine echtzeitfähige Schätzung zu erlauben, wurde daher in [Wil14] 
eine Aufteilung des Faktor-Graphen in eine Echtzeitkomponente und eine 
Komponente für das Schließen von Schleifen vorgestellt. Während erstere 
Komponente ein echtzeitfähiges Filter ist, läuft die andere Komponente 
parallel im Hintergrund und optimiert den ganzen Pfad inklusive Schlei- 
fenschlüssen mit iSAM2. 

In [Zha14b] wurde ein Verfahren für LiIDAR-Odometrie und Kartierung 
vorgestellt, welches Laserscans als einzige Sensordaten verwendet, wobei 
für die Bewegungsschätzung ein Scan-Matching anhand von extrahierten 
Merkmalspunkten von Linien und Ebenen zum Einsatz kommt. In [Sha18b] 
wurde es um das Schließen von Schleifen mittels ICP-Algorithmus (eng- 
lisch: iterative closest point - ICP) erweitert, wobei für die finale Schätzung 
des Pfades für die Kartierung ebenfalls das o. g. iSAM2 zum Einsatz kommt. 
In [Ye17] wurde ein Verfahren vorgestellt, welches für das Scan-Matching 
eine Punkt-zu-Ebene-Variante des ICP-Algorithmus anwendet und für die 
Bewegungsschätzung mit den Daten einer IMU fusioniert, wobei auch im 
Vergleich mit dem Verfahren aus [Zha14b] gezeigt werden konnte, dass mit 
den zusätzlichen Daten der IMU eine robustere und präzisere Kartierung 


erzielt werden kann. 
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2.2.2 Kartenerstellung 


Eine Karte dient dem mobilen Roboter als Modell der Umgebung für die 
Navigation. Die Art der erstellten Karte ist abhängig von dem auf dem 
mobilen Roboter verwendeten perzeptorischen Sensor und der gegebe- 
nenfalls vorhandenen Vorverarbeitung der Sensordaten. Die Karte kann 
aus unverarbeiteten Sensordaten, wie z.B. Punktwolken, bestehen oder 
aber die Punkte beispielsweise in einem Raster aggregieren. Mit diesen 
Arten von Karten erhält man eine dichte Repräsentation der Umgebung. 
Es können jedoch auch Landmarken aus den Sensordaten extrahiert wer- 
den und die entstehenden Karten bestehen folglich aus einem Tupel von 


Landmarken. 


2D-Sensoren 


Die Nutzung von Rasterkarten für 2D-SLAM wurde 1987 in [EIf87] als 
Belegtheitskarte (englisch: occupancy grid map) eingeführt und das Kon- 
zept wurde in vielen späteren SLAM-Verfahren genutzt [Häh03; Eli03; 
Gri05; Koh11; Shal8a]. Die Fläche wird dabei in ein gleichmäßiges Ras- 
ter unterteilt und jede Zelle enthält eine Belegtheitswahrscheinlichkeit. 
Die Modellierung mit einer Belegtheitswahrscheinlichkeit hat den Vorteil, 
dass sowohl die Information über Objekte in der Umgebung als auch über 
den freien Raum abbildbar ist. Diese Art der Karte lässt sich mit Sensoren 
erstellen, welche Entfernungen zu Objekten messen, wie einem Array aus 
Sonarsensoren oder einem 2D-LiDAR. 

In [Lu97] wurden unverarbeitete Punktwolken für die Kartierung ver- 
wendet. Wie in Abschnitt 2.2.1 dargestellt, handelt es sich hier um eine 
graph-basierte Methode, bei der jedoch keine explizite Karte aufgebaut 
wird. In den darauf aufbauenden Arbeiten wurden für das Schließen von 
Schleifen temporäre Rasterkarten erzeugt, um mit diesen durch Korrela- 
tion die beste Übereinstimmung des aktuellen Kartenteils mit einem bereits 
besuchten Teil zu finden [Gut99; Gut00]. 
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Weit verbreitet sind auch Karten aus Punktlandmarken, welche in ver- 
schiedenen Veröffentlichungen der auf Bayes-Filter basierenden Algorith- 
men, wie EKF-SLAM und FastSLAM bzw. FastSLAM 2.0, Verwendung fan- 
den [Dis01; Dur06a; Mon02; Mon03b]. Die Punktlandmarken sind aus Sens- 
ordaten extrahiert und lediglich über ihren Ort definiert, was sie anfällig 
für Fehlzuordnungen macht. 

Neben den Karten aus Punktlandmarken lassen sich auch Rasterkar- 
ten mit Partikelfiltern kombinieren. In [Häh03] wurde die Kartierung einer 
Belegtheitskarte mit dem FastSLAM-Algorithmus vorgestellt. Da jedes Par- 
tikel seine eigene Kartenschätzung besitzt, müssen diese im Speicher gehal- 
ten und beim Resampling mit kopiert werden. Um hier den Speicher- und 
Kopieraufwand zu verringern, wurde in [Eli03] eine effiziente Kartenreprä- 
sentation eingeführt, bei der nicht für jedes Partikel eine komplette Karte 
vorgehalten werden muss. Stattdessen wurde eine Datenstruktur einge- 
führt, bei der nur eine einzige Karte existiert, aber dafür in jeder Zelle eine 
Baumstruktur gespeichert ist, die die Information enthält, welche Partikel 
die Belegtheit in der jeweiligen Zelle verändert haben. Eine weitere Mög- 
lichkeit, den Aufwand zu verringern, wurde durch die Verwendung von 
FastSLAM 2.0 erzielt, wobei die notwendige Partikelanzahl im Vergleich 
zu [Häh03] wesentlich verringert werden konnte [Gri05; Gri07]. 


3D-Sensoren 


Sensoren wie 3D-LiDAR oder Tiefenkameras liefern 3D-Punktwolken und 
für deren Kartierung sind die Anforderungen weitaus höher, da mehr 
Freiheitsgrade zu schätzen sind und die Datenmenge im Vergleich zu 2D- 
Sensoren sehr viel größer ist. Folglich sind auch die Speicheranforderungen 
für die 3D-Karten höher. 

Für 3D-SLAM werden häufig graph-basierte Methoden eingesetzt, bei 
denen keine explizite Karte aufgebaut wird, sondern Randbedingungen von 
paarweisem Scan-Matching zwischen Punktwolken in den Graph einge- 
fügt werden [Nüc09]. Für das Scan-Matching wird oft der ICP-Algorithmus 
herangezogen [Sur03; Kat06; Nüc07]. Als punkt-basierte Methode sind 
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keine Annahmen über die Struktur der Umgebung und die anzutreffenden 
Objekte notwendig, weshalb es sich auch besonders für unstrukturierte 
Außenumgebungen eignet [Rus01]. Es existieren weitere Varianten des 
ICP-Algorithmus, wie z.B. Punkt-zu-Fläche-Registrierung, um das Scan- 
Matching in strukturierten Umgebungen genauer zu machen, wodurch 
jedoch die Präzision in unstrukturierten Umgebungen verschlechtert wird 
[Pom13]. Eine Variante, welche durch eine lokale probabilistische Flächen- 
modellierung für heterogene Umgebungen geeignet ist und speziell für 
Punktwolken mehrlagig scannender Sensoren entwickelt wurde, ist der 
generalisierte ICP-Algorithmus (Generalized-ICP - GICP) [Seg09]. 

Das Scan-Matching mit Punktwolken ist nur eine implizite Kartierung 
und die konkrete Karte muss in einem Nachverarbeitungsschritt aufgebaut 
werden. Dies kann beispielsweise als Punktwolkenkarte erfolgen, was eine 
Aggregation aller Punktwolken bedeutet. Wenn über lange Zeit kartiert 
wird, ist dies jedoch mit einem enormen Speicheraufwand verbunden, da 
die Anzahl der Punkte mit der Zeit ansteigt. Außerdem wird die Karte an 
mehrfach besuchten Orten eine sehr viel höhere Punktdichte aufweisen 
als an seltener besuchten Orten. Um den Speicheraufwand zu verringern 
und für eine gleichmäßigere Punktdichte zu sorgen, kann die Punktwol- 
kenkarte anschließend räumlich ausgedünnt werden. Es existieren jedoch 
auch andere speichereffizientere Methoden, welche im Folgenden vorge- 
stellt werden. 

Eine sehr kompakte Repräsentation ist eine gerasterte Höhenkarte, 
welche in einem 2D-Raster in jeder Zelle einen Höhenwert speichert. Mit 
dieser Art von Karte lassen sich Oberflächen modellieren, jedoch keine 
Durchgänge oder überhängenden Objekte. In [Tri06] wurde deswegen eine 
Erweiterung der Höhenkarte vorgestellt, welche für jede Zelle mehrere 
Höhenlevel speichert und so auch mehrere Ebenen und vertikale Struktu- 
ren modellieren kann. 

Ein anderer Ansatz basiert auf einer Karte, welche eine 3D-Rasterung 
der Umgebung enthält und für jede belegte Zelle eine lokale Normalvertei- 


lung der darin enthaltenen Punkte schätzt, was als Normal Distributions 
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Transform (NDT) bezeichnet wird [Mag07]. Die Zellen sind kubische Volu- 
menelemente und werden daher auch als Voxel bezeichnet (von englisch 
volume element in Anlehnung an Pixel für picture element). Diese Art der 
Karte erlaubt eine sehr kompakte 3D-Repräsentation, wobei durch die Nor- 
malverteilungen die Formen der Oberflächen in der Umgebung modelliert 
sind, weshalb die Elemente der Karten auch manchmal als Surfel (kurz 
für englisch: surface element) bezeichnet werden [Klä12]. In [Sto13] wurde 
gezeigt, dass in einigen Umgebungen eine NDT-Karte mit vielfach gerin- 
gerer Auflösung äquivalent zu einer reinen Belegtheitskarte ist und dabei 
auch die Aktualisierung wesentlich weniger Rechenaufwand benötigt. 

Die Rasterung der Umgebung muss nicht zwangsläufig regelmäßig 
sein, sondern kann auch hierarchisch erfolgen. Die in [Wur10] vorgestellte 
und Octomap genannte Karte verwendet eine Octree-basierte Unterteilung 
der Umgebung, welche in jedem Knoten ein kubisches Volumen repräsen- 
tiert, das ebenfalls als Voxel bezeichnet wird. Jedes Voxel beinhaltet eine 
Belegtheitswahrscheinlichkeit wie die in Abschnitt 2.2.2 erläuterten 2D- 
Belegtheitskarten und kann somit auch Informationen über freien Raum 
beinhalten. 

In [Moo11] besteht die Karte aus Ebenen, welche aus den Punktwol- 
ken extrahiert werden. Die Karte ist dabei in ein 3D-Raster aufgeteilt und 
jede Zelle beinhaltet maximal eine Ebene. Die aus der aktuellen Punkt- 
wolke extrahierten Ebenen werden für die Lokalisierung in der Karte ver- 
wendet, indem eine Registrierung der Ebenen mit einer Variante des ICP- 
Algorithmus durchgeführt wird. Da in diesem Ansatz nur Ebenen für die 
Kartierung und Registrierung verwendet werden, ist er hauptsächlich in 


urbanen oder anderen strukturierten Umgebungen anwendbar. 


Kameras 


In der Literatur wird bei der Verwendung von bildgebenden Sensoren 
von Visual-SLAM gesprochen. In den merkmalsbasierten Visual-SLAM- 
Verfahren werden zunächst in einem Vorverarbeitungsschritt Merkmale 
aus den Bildern extrahiert, welche dann als Landmarken dienen. Die meis- 
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ten Arbeiten verwenden lokale Bildmerkmale und es existieren Veröffent- 
lichungen mit verschiedenen lokalen Merkmalen, wie z. B. SIFT-Merkmale 
in [Sim05; Eli06; Gil06], SURF-Merkmale in [Ste08] oder ORB-Merkmale 
in [Mur15]. Da in den meisten Umgebungen viele dieser Merkmale in den 
Bildern gefunden werden, wird auch eine große Menge an Landmarken 
in die Karte eingetragen. Die Dichte der resultierenden Karten liegt zwi- 
schen den dünnbesetzten Landmarkenkarten und den dichten Karten aus 
LiDAR-Daten. 

Neben den merkmalsbasierten Visual-SLAM-Verfahren existieren auch 
direkte Methoden wie beispielsweise LSD-SLAM, das direkt auf den Inten- 
sitätsbildern arbeitet und eine semi-dichte Punktwolkenkarte aus Tiefen- 
bildern konstruiert [Eng14]. 


2.2.3 Wiedererkennung 


Die Wiedererkennung spielt in SLAM in verschiedenen Kontexten eine 
große Rolle. So ist bei Landmarkenkarten die Wiedererkennung bereits kar- 
tierter Landmarken eine wichtige Voraussetzung für die Erstellung einer 
konsistenten Karte. Schon wenige Fehlzuordnung können zum Scheitern 
des SLAM-Algorithmus führen. Außerdem müssen bereits besuchte Orte 
wiedererkannt werden, um Schleifen zu schließen. Auch hier kann schon 
eine nicht oder falsch erkannte Schleife zu einer inkonsistenten Karte füh- 
ren. 


Landmarken 


Die Wiedererkennung im Kontext der Landmarken wird auch Datenzuord- 
nung genannt, da hier die aktuelle Beobachtung einer bereits kartierten 
Landmarke zugeordnet werden muss. Die Datenzuordnung ist ein schon 
lange bekanntes Problem in der Literatur über Zielverfolgung (englisch: 
target tracking) und das einfachste Verfahren ist die Maximum-Likelihood- 
Datenzuordnung, auch bekannt als Zuordnung des nächsten Nachbarn 
(englisch: nearest neighbor - NN) [Mon07]. Diese einfache Zuordnung mit- 
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tels Maximum Likelihood (ML) kann sehr effektiv sein, wenn die Land- 
marken nicht zu dicht beieinander liegen. Sind jedoch Mehrdeutigkeiten 
vorhanden, können diese leicht zu Fehlzuordnung führen. 

Liegen Landmarken dicht beieinander, kann dies vor allem bei der 
Verwendung von Punktlandmarken leicht zu Fehlzuordnungen führen, da 
diese nur über ihren Ort definiert sind. Nahe beieinanderliegende Land- 
marken werden jedoch sehr wahrscheinlich auch gleichzeitig beobachtet. 
Anstatt die beobachteten Landmarken einzeln sequenziell zuzuordnen, ist 
es sinnvoll, diese gemeinsam zuzuordnen, wofür verschiedene Verfahren 
entwickelt wurden [Nei01; Bai02]. In [Ram07] wurde eine Methode vorge- 
stellt, welche ein erweitertes Landmarkenmodell verwendet, und gezeigt, 
dass damit eine robustere Wiedererkennung erzielt werden kann. Das ein- 
fache Punktlandmarkenmodell aus Daten eines 2D-LiDARs wird dabei um 
eine aus Kameradaten extrahierte, ansichtsbasierte Komponente erweitert. 

Mit Partikelfiltern werden mehrere Kartenhypothesen und damit auch 
implizit individuelle Datenzuordnungshypothesen verfolgt. Dies kann 
sogar noch erweitert werden, indem nicht immer die am besten passende 
Zuordnung verwendet wird, sondern die Zuordnungen zufällig mit einer 
Wahrscheinlichkeit proportional zu ihrem Likelihood gezogen werden, 
was als Monte-Carlo-Datenzuordnung in [Mon03a] vorgestellt wurde. 
Eine weitere Methode wurde in [Nie03] vorgestellt, bei der im Falle von 
Mehrdeutigkeiten für jede alternative Hypothese ein zusätzliches Partikel 
zum Filter hinzugefügt wird. Damit die Anzahl von Partikeln nicht stetig 
anwächst, wird deren Anzahl beim nächsten Resampling wieder auf die 
ursprüngliche Anzahl reduziert. 

Da bei dichten Karten keine Landmarken im eigentlichen Sinne vor- 
handen sind, kommen hier Scan-Matching-Verfahren zum Einsatz, welche 
die relative Position und Ausrichtung der aktuellen Beobachtung zur Karte 
oder zu einer vorangegangenen Beobachtung bestimmen sollen. Hierbei 
handelt es sich beispielsweise um Gradientenabstiegsverfahren oder den 


ICP-Algorithmus in verschiedenen Varianten. Es kann auch hier zu Feh- 
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lern kommen, da es sich zum einen um lokale Verfahren handelt und zum 


anderen auch die Umgebung mehrdeutig sein kann [Gri07; Pom13; Sta08]. 


Schleifen 


Wenn der mobile Roboter für einige Zeit unbekanntes Gebiet erkundet 
und wieder an einen bekannten Platz zurückkehrt, muss dies anhand der 
aufgezeichneten Karte wiedererkannt werden. Dies wird in der SLAM- 
Literatur in Englisch als loop closure bezeichnet, also das Schließen von 
Schleifen, und ist eine der größten Herausforderungen, da die Bewegungs- 
unsicherheit nach der Befahrung unbekannten Gebietes besonders groß 
ist und sich die im vorherigen Abschnitt beschriebene Wiedererkennung 
von Landmarken damit als besonders schwierig erweist [Mon07]. 

Bei Landmarkenkarten ist die Gefahr für Fehlzuordnungen beim Schlie- 
ßen von Schleifen besonders hoch. Deshalb wurden verschiedene und zum 
Teil sehr aufwändige Verfahren entwickelt, um bereits besuchte Plätze 
wiederzuerkennen. In [Ho05] wurde ein Ansatz mit mehreren Sensoren 
verfolgt. So werden als Landmarken SIFT-Merkmale aus Kamerabildern 
mit räumlichen Deskriptoren von Segmenten von 2D-Laserscans kombi- 
niert, wobei diese Landmarken ausschließlich für das robuste Detektieren 
von Schleifen verwendet werden. Das im vorherigen Abschnitt bereits 
vorgestellte Verfahren von [Ram07] verwendet ebenfalls Landmarken aus 
LiDAR- und Kameradaten und es konnte gezeigt werde, dass damit Schlei- 
fen zuverlässiger geschlossen werden können. 

Bei dichten 2D-Karten aus Punktwolken wurden für das Schließen von 
Schleifen in [Gut99], wie in Abschnitt 2.2.2 bereits beschrieben, zusätzli- 
che Rasterkarten erzeugt, um mit diesen durch Korrelation die beste Über- 
einstimmung des aus aktuellen Beobachtungen erzeugten Kartenteils mit 
bereits besuchten Teilen der Karte zu ermitteln. Bei den 2D-Rasterkarten 
kommt für das Schließen von Schleifen ebenfalls das Scan-Matching zum 
Einsatz [Gri05]. In [Nüc07] werden 3D-Punktwolkenkarten erzeugt und 


der Schleifenschluss erfolgt über ein Abstandsmaß in Kombination mit 
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dem ICP-Algorithmus, welcher auch für das Scan-Matching der aufeinan- 
derfolgenden Punktwolken Verwendung findet. 

In [Koh11] wird EKF-SLAM mit einer 2D-Rasterkarte durchgeführt 
und es wird gezeigt, dass die Genauigkeit ausreicht, um kleinere Schleifen 
zu schließen, ohne dass potentielle Mehrdeutigkeiten gesondert behandelt 
werden müssen. 


2.2.4 Kombination mit Multi-Sensor-Fusion 


Für die meisten SLAM-Methoden ist lediglich ein Sensor für die Messung 
der Bewegung und ein Sensor zur Perzeption der Umgebung notwendig, 
weshalb die Fusion weiterer Sensoren in nur wenigen Veröffentlichungen 
eine Rolle spielt. So wurde in [Fre10] angemerkt, dass in den bisherigen 
Forschungen zu SLAM der Fokus auf die Abwesenheit von absoluten Sen- 
soren wie GPS und wenig auf reale Anwendungen gelegt wurde. 

In [Gui02a] wurde ein hochgenaues RTK-GPS? neben Rad-Odometrie 
und 2D-LiDAR als zusätzlicher Sensor in einem EKF-SLAM-Algorithmus 
verwendet und es konnte gezeigt werden, dass es als absoluter Sensor zu 
einer Reduzierung der Unsicherheiten in der Lokalisierung und der Karte 
beiträgt. 

Auch in [Thr06a] kam ein zusätzliches GPS zum Einsatz, wobei hier 
ein Graph-SLAM-Verfahren verwendet wurde. Auch hier konnte das GPS 
zu einer Verbesserung der Präzision der Lokalisierung und Kartierung bei- 
tragen und dies selbst wenn das GPS nur sporadisch verfügbar war. 

Eine Integration eines hochgenauen RTK-GPS in einen RBPF-SLAM- 
Algorithmus wurde in [Shal8a] vorgestellt. Dabei werden die GPS-Mes- 
sungen über die Partikelgewichte fusioniert und es konnte auch hier eine 


präzisere und konsistentere Kartierung erzielt werden. 


2. RTK steht im Englischen für real-time kinematics und bezeichnet ein Verfahren, mit dem 
mittels zusätzlicher Korrektursignale von einer Referenzstation im Idealfall Genauigkei- 
ten im Zentimeterbereich erzielt werden können. 


34 


Kapitel 3 


Theoretische Grundlagen 


Im Folgenden werden die theoretischen Grundlagen der bestehenden 
Methoden zur Multi-Sensor-Fusion und SLAM erläutert, auf denen diese 
Arbeit aufbaut. Es werden an den entsprechenden Stellen Verweise auf die 
in dieser Arbeit entwickelten Methoden angegeben, um sie in Kontext zu 


den bestehenden Methoden zu setzen. 


3.1 Multi-Sensor-Fusion 


Die in dieser Arbeit verwendeten Methoden zur Multi-Sensor-Fusion basie- 
ren auf Bayes-Filtern. Diese erlauben durch ihre probabilistische Modellie- 
rung eine Fusion unter Berücksichtigung der jeweiligen Unsicherheiten. 

Ein rekursives Bayes-Filter basiert auf der Markov-Annahme, d. h., dass 
zum Zeitpunkt k alle Information in dem aktuellen Zustand x, und in der 
aktuellen Messung z% vorhanden ist. Der aktuelle Zustand x; ist bedingt 
unabhängig von allen weiter zurückliegenden Zuständen gegeben dem 
vorherigen Zustand xx-ı und die aktuelle Messung zę ist bedingt unab- 
hängig von allen zurückliegenden Messungen und Zuständen gegeben dem 
aktuellen Zustand x; [Sar13]. 
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3.1.1 Kalman-Filter 


Das Kalman-Filter gehört zu den rekursiven Bayes-Filtern und ist ein opti- 
maler Zustandsschätzer für lineare Systeme im Sinne eines minimalen 
quadratischen Fehlers [Sim06]. 

Es wird ein lineares dynamisches Modell angenommen, bei dem der 
Zustand x; zum Zeitpunkt k aus dem Zustand zum Zeitpunkt k — 1 her- 


vorgeht: 
Xk = FkXk-1 + Brug + we, (3.1) 


mit dem Zustandsübergangsmodell Fg, der optionalen Eingabe ur und 
deren Dynamik Bx. Das System ist durch ein additives Systemrauschen 
wx gestört, welches als normalverteilt angenommen wird: 


wg ~ N(0,Qx). (3.2) 


Der wahre Zustand x; wird zum Zeitpunkt k durch die Messung zx beob- 
achtet: 


Zk = AXE tr, (3.3) 


mit dem Messmodell H; und dem additiven Messrauschen rg, welches 


ebenfalls als normalverteilt angenommen wird: 
rg ~ N(0, Rx). (3.4) 


Da der wahre Zustand x selbst nicht beobachtbar ist, wird anhand 
von Beobachtungen die Zustandsschätzung £ in rekursiven Filterschritten 
berechnet. Dabei kann ein Filterschritt des Kalman-Filters in zwei Pha- 
sen unterteilt werden. Die erste Phase wird Prädiktion genannt und bil- 
det zum Zeitpunkt k die A-priori-Zustandsschätzung Sur, indem die A- 
posteriori-Zustandsschätzung Xx-1|x-1 des vorangegangenen Zeitschritts 
k — 1 propagiert wird. Die Propagation erfolgt dabei mit der Zustands- 
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übergangsmatrix Er und optional der zusätzlichen Eingabe u; und deren 
Dynamik Bx: 


Suz Fr&k-ır-ı + Brug . (3.5) 


Die A-priori-Systemkovarianzmatrix Px-1jk-ı wird ebenfalls propa- 
giert und es wird ein additives Systemrauschen mit der Kovarianzmatrix 


Or hinzugefiigt, welches die Modellunsicherheiten beriicksichtigt: 


Prir-ı = PP AE + Qk. (3.6) 


Die zweite Phase beinhaltet die Aktualisierung durch eine Messung 
zt und wird Korrekturschritt genannt. Die Messung wird entsprechend 
der Unsicherheiten mit der A-priori-Zustandsschätzung X,|4-1 kombiniert 
und das Resultat ist eine verbesserte A-posteriori-Zustandsschatzung Xx |x. 
Dabei wird zunächst die prädizierte Zustandsschatzung %;|x-ı mittels der 
Messmatrix H; in den Messraum abgebildet. Aus der Differenz zur eigent- 
lichen Messung z; wird das sogenannte Residuum y; gebildet, welches 
mit der Kalman-Gain-Matrix Kr gewichtet die A-priori-Schätzung kor- 
rigiert. In die Kalman-Gain-Matrix Kg gehen dabei neben der A-priori- 
Systemkovarianzmatrix P;jx-ı und der Messmatrix Hr die Kovarianzma- 


trix der Messunsicherheit R; ein. 


Ük = Zr — Hı&kır-ı (3.7) 
Ke = Pur HI (HrPei HI + Re) (3.8) 
Sit = Sit +Kıyk (3.9) 


Für die Berechnung der A-posteriori-Systemkovarianzmatrix P; |, gibt 


es verschiedene Formulierungen. Bei der Folgenden handelt es sich um die 
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Joseph-Form, welche durch ihre Symmetrien besonders stabil gegenüber 


numerischen Ungenauigkeiten ist [Gre14]: 


Pur = (I- KkHk)Prix-ı(I-KrHk)" + Ke RK; , (3.10) 


mit I als Einheitsmatrix. 

Während die Prädiktion in jedem Zeitschritt genau einmal ausgeführt 
wird, können mehrere gleichzeitige Messungen in mehreren Korrektur- 
schritten integriert werden. Es kann aber auch vorkommen, dass keine 
Messungen vorhanden sind und lediglich prädiziert wird, wobei dann 
natürlich die Unsicherheit immer weiter anwächst. 

Abbildung 3.1 zeigt ein dynamisches bayessches Netz (DBN) eines 
Kalman-Filters, aus dem die eingangs erwähnte Markov-Annahme deutlich 
wird. Das dynamische bayessche Netz findet hier als graphisches Modell 
Verwendung, um die Zustände und deren Abhängigkeiten über die Zeit 
zu visualisieren [Rus09]. Die Zustände zum Zeitpunkt k sind mit xx in 
Hellblau dargestellt und die Messungen zx in Orange. Der wahre Zustand 
selbst ist nicht direkt beobachtbar, sondern kann nur anhand der Beobach- 
tungen geschätzt werden. Da der wahre Zustand somit verborgen ist, wird 
es im Englischen als hidden Markov model (HMM) bezeichnet [Rus09]. 


2000 
©0000 o 


Abbildung 3.1: DBN eines Kalman-Filters. 
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3.1.2 Erweitertes Kalman-Filter 


Das erweiterte Kalman-Filter (EKF) ist eine Variante für nichtlineare Sys- 
teme und basiert auf der Linearisierung der Filtergleichungen um den aktu- 
ell geschätzten Zustand [Sär13]. 

Im Prädiktionsschritt zum Zeitpunkt k wird die A-priori-Zustands- 


schätzung Xx|jx-ı wie folgt berechnet: 


&rır-ı = f (Ke—-1]k-1, Uk) » (3.11) 


wobei f eine nichtlineare Funktion ist, welche die Schätzung des prädi- 
zierten Zustands aus dem vorhergehenden Zustand und der Eingabe ug 
berechnet. 

Die Berechnung der Systemkovarianzmatrix Du erfolgt wie beim 
Kalman-Filter mit 


Pkjk-1 = FrPr-ıR-ı FL + Or, (3.12) 


Verglichen mit Gleichung (3.6) beim linearen Kalman-Filter hat sich die 
Berechnung nicht geändert, jedoch ist F, hier die Jacobi-Matrix von f 
bezüglich SL vu Uk: 


en 


F 
k~ äs 


(3.13) 
Êk-ijk-1-Uk 
Der Korrekturschritt integriert auch hier die Messung z; und berechnet 


die A-posteriori-Zustandsschätzung Sur. 


Ye = 2k - h(&kı-ı) (3.14) 
Kr = Bu HIH Pee + Rx) (3.15) 
Sur = Kkjr-ı + Kr (3.16) 


Bei der Berechnung des Residuums yx ist nun eine nichtlineare Funk- 
tion h zur Ermittlung der prädizierten Messung auf Basis der A-priori- 
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Zustandsschätzung X,|x-1 vorhanden und Hy, ist deren Jacobi-Matrix 
bezüglich Xkk-1! 
oh 
H; = — (3.17) 
Ox 


Zut 
Die Berechnung der A-posteriori-Systemkovarianzmatrix Du kann 


wieder identisch zum linearen Kalman-Filter erfolgen: 


Du = (I- Kx Hx) Pxx-1 (1 — K;H,)" + KRK} x (3.18) 


3.1.3 Stochastisches Klonen 


Wenn mehrere relative Messungen vorhanden sind, ist die in Abschnitt 
3.1.1 vorgestellte Formulierung des Standard-Kalman-Filters nicht mehr 
ausreichend. Während ein einzelner relativ messender Sensor in die Prädik- 
tion eingehen kann, ist die Integration weiterer relativer Messungen nur 
über den Korrekturschritt möglich. Bei relativen Messungen hängt jedoch 
der aktuelle Zustand von einem vorhergehenden Zustand ab und verletzt 
damit die Markov-Annahme. Um eine zusätzliche relative Messungen for- 
mal korrekt in ein Kalman-Filter zu integrieren, stellten Roumeliotis et al. 
die Methode des sogenannten stochastischen Klonens (englisch: stochastic 
cloning - SC) mit einem Kalman-Filter (SC-KF) vor, welche den Zustand 
mit einem Klon des zu einer relativen Messung zugehörigen Zustands 
erweitert [Rou02]. 

Abbildung 3.2 zeigt ein DBN mit relativen Messungen Zk,k+m zwischen 
den zwei Zuständen zu den Zeitpunkten k und k + m für m = 1, welche in 
Hellgrün dargestellt sind. Darin wird deutlich, dass durch Abhängigkeiten 
von vorangegangenen Zuständen die Markov-Annahme verletzt wird. 

Für eine relative Messung Zk,k+m zwischen den zwei Zuständen zu den 


Zeitpunkten k und k + m wird die Zustandsschätzung um einen Klon von 
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Abbildung 3.2: DBN mit relativen Messungen. 


Zu erweitert und man erhält die augmentierte Zustandsschätzung 


al Zu 


7 (3.19) 


Zum Zeitpunkt k + m ergibt sich dann nach m Prädiktionen die aug- 


mentierte Zustandsschätzung 


u Zut 
Xk+m|k = i | d (3.20) 
+m 


Weil Su ein Klon der zurückliegenden Zustandsschatzung ist, die durch 
die Prädiktion unverändert bleibt, wurde sie in [Rou02] als statisch bezeich- 
net. Im Gegensatz dazu wird Sit im Filter durch die Prädiktion propa- 
giert und wurde deshalb als sich entwickelnde Zustandsschätzung bezeich- 
net. 

Die Systemkovarianzmatrix P wird ebenfalls erweitert und hat zum 
Zeitpunkt k die Form 


Du Prix 


(3.21) 
Pre Pax 


Du = 
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Die linke obere Sub-Matrix der augmentierten Systemkovarianzmatrix 
P entspricht dabei dem statischen Teil der Zustandsschätzung und bleibt 
ebenfalls unverändert durch die Prädiktion: 


E E (3.22) 
kalk = | 9 ae Elo FT, 0 ol ; 


Nach m Prädiktionsschritten wird die augmentierte Systemkovarianz- 


matrix P dann zu 


Prise E Tout (3.23) 
Fram kPk\|k Prrmik 


mit dem kumulativen Produkt der Zustandsübergangsmatrizen 


m 
unn = FeamEkim—1-Fes2Fee = | | Fren. (3.24) 


i=1 


Wird nach m Schritten eine relative Messung integriert, ist deren Resi- 


duum 


Vum = Zk,ktm — Èk kim > (3.25) 


wobei Êk k+m die Differenz der prädizierten Messungen h(Xx|jx) und 
h(Xk+m|x) ist und Zk k+m die relative Messung zwischen den Zuständen 
zu den Zeitpunkten k bzw. k + m. D. h., die Prädiktion ist im Vergleich zu 
der Messung mit einer m-fachen Rate verfügbar. 

Die augmentierte Kalman-Gain-Matrix Kiam kann analog zum Standard- 


Kalman-Filter berechnet werden: 
z < 4T x-1 
Kırm = Premk HkimSk+m H (3.26) 


mit 
< ` < ST 
Skim = Hg+mPkim|kH gam + Bt Kaum, (3.27) 
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wobei Rx ,x+m die Messfehlerkovarianzmatrix der relativen Messung ist. 


Die augmentierte Messmatrix 
Ham = [He Hirn] (3.28) 


beinhaltet beide Jacobi-Matrizen, welche bezüglich der jeweiligen Zustands- 
schätzungen Xx |x bzw. Xx+|k gebildet werden. 

Die resultierende augmentierte Kalman-Gain-Matrix Kram (3.26) ist 
ebenfalls eine Zusammensetzung aus den Kalman-Gain-Matrizen für die 
statische und die sich entwickelnde Zustandsschatzung: 


(3.29) 


In [Rou02] wurde weiterhin argumentiert, dass die relative Messung 
keine neue Information bezüglich des geklonten Zustands x; liefert, sodass 
Kr von Gleichung (3.29) 0 ist und 


Kian (3.30) 


Kırm 


gilt. 
Folglich wird die aktuelle Schätzung des Zustands und die Systemko- 
varianzmatrix nur mit der unteren Sub-Matrix Kg+m von Gleichung (3.29) 


berechnet: 


Xktm|ktm = Xkım|k + KktmYkım: (3.31) 


Pirmjkim = Pisani — KeemStemK pen: (3.32) 


Es wird deutlich, dass für eine einzelne zusätzliche Korrektur mittels 
stochastischem Klonen die augmentierte Systemkovarianzmatrix P nicht 
explizit durch alle Filterschritte propagiert werden muss, sondern zum 
Zeitpunkt k + m rekonstruiert werden kann. Hierfür müssen lediglich die 
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vorangegangene Systemkovarianzmatrix Du und alle seither angewen- 
deten Zustandsübergangsmatrizen Fx+1...Fx+m in den Gleichungen (3.23) 
und (3.24) verrechnet werden. Zusammen mit der geklonten Zustands- 
schätzung Sur, welche zur Berechnung der prädizierten Messung Zx,k+m 
notwendig ist, können alle nachfolgenden Gleichungen (3.25) bis (3.32) 
berechnet werden, um die A-posteriori-Zustandsschätzung Xx+m|k+m und 
deren Systemkovarianzmatrix Pk+m|k+m zu erhalten. 

Auch in [Mou07] wurden zwei relative Sensoren fusioniert, aber die 
Integration weiterer Sensoren wurde als mögliche Erweiterung diskutiert. 
Die Vereinfachung der Berechnung der augmentierten Systemkovarianz- 
matrix Primi k mittels der Gleichungen (3.23) und (3.24) ist demnach nicht 
mehr gültig. Stattdessen muss die Zustandsschätzung tatsächlich geklont 
und die augmentierte Systemkovarianzmatrix P explizit durch alle Filter- 
schritte propagiert werden. Hierzu konnte jedoch keine weiterführende 
Literatur ermittelt werden. 

Es existieren lediglich - soweit bekannt - Veröffentlichungen zu 
Methoden, bei denen bei der Integration weiterer Sensoren nur ein Teil der 
Zustandsschätzung geklont wird, wobei die teil-augmentierte Zustands- 
schätzung und Systemkovarianzmatrix in alle Filterberechnungen einge- 
hen [Chi11; Kle11; Ste12]. Die partiell augmentierte Zustandsschätzung 


zum Zeitpunkt k + m ergibt sich dann zu 


E KT 
Xk+m|k = b i , (3.33) 
+m 


wobei Sr der Teil des Zustandsraums ist, welcher von der relativen 
Messung betroffen ist und geklont wird, was mit der Kennzeichnung mit 
c verdeutlicht wird. Die partiell augmentierte Systemkovarianzmatrix hat 


zum Zeitpunkt k die Form 


Se 
Datt P. ut 


(3.34) 
Paczke Pr 


Du = 
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wobei es sich bei D. tt um eine dim(x) x dim(X,)-Matrix handelt und a 
für den Teil des aktuellen Zustands steht. Bei dim(x) handelt es sich um 
die Dimension des Zustandsraums und bei dim(x.) um die Dimension des 
geklonten Teils des Zustandsraums. 

In Abschnitt 5.2 wird eine Realisierung der von [Mou07] vorgeschla- 
genen Erweiterung auf mehrere Sensoren vorgestellt und eine neuartige 
alternative Methode präsentiert, bei der dennoch eine Rekonstruktion der 


augmentierten Systemkovarianzmatrix ermöglicht wird. 


3.2 Simultane Lokalisierung und 
Kartenerstellung 


Bei der simultanen Lokalisierung und Kartenerstellung sind die Unsicher- 
heit in der Lokalisierung und die Fehler in der Karte voneinander abhän- 
gig. Das SLAM-Problem und seine Abhängigkeiten können als Zufalls- 
variablen modelliert und wie die Zustandsschätzung des Kalman-Filters 
als dynamisches bayessches Netz (DBN) visualisiert werden, siehe Abbil- 
dung 3.3 [Thr05]. Die Posen des Roboters x; sind in Hellblau dargestellt, 
wobei k die Zeitschritte sind. Die Messungen eines Lokalisierungssensors 
sind in Rot als Eingabe u; dargestellt. Die Messungen eines perzeptori- 
schen Sensors zx sind in Orange dargestellt und die mit diesem beobach- 
teten Landmarken A; der Umgebung in Grün. Die Datenzuordnung ng 
repräsentiert die Zuordnung der Beobachtungen zu den bereits kartierten 
Landmarken und ist eine sehr wichtige Voraussetzung, da erst die wieder- 
holte Beobachtung von Landmarken und deren Korrektur in der Karte die 
Genauigkeit der Karte und damit auch der Lokalisierung verbessert. Falsch 
zugeordnete Landmarken können hingegen zu Inkonsistenzen oder sogar 
zu einem Scheitern der Kartierung führen. 
Die Lösung des SLAM-Problems ist dann die Schätzung des vollen 
SLAM-Posteriors 
p(x*, AS uk, n) , (3.35) 
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Abbildung 3.3: Das SLAM-Problem als DBN (nach [Thr05]). 


2 


0-0-0-0 


wobei das hochgestellte k bedeutet, dass hiermit die komplette Sequenz 
bis zum Zeitpunkt k eingeschlossen ist, und A als Karte alle Landmarken 
A enthält. 

Damit die Schätzung des SLAM-Problems zu einer besseren Lösung 
konvergiert, sind bei der wiederholten Beobachtung bereits besuchter Kar- 
tenteile mehrere Kriterien ausschlaggebend. Das erste Kriterium ist die Ein- 
beziehung der Abhängigkeiten zwischen den Posen des mobilen Roboters 
und den Landmarken sowie der Abhängigkeiten der Landmarken unter- 
einander. Das zweite Kriterium ist eine robuste Datenzuordnung, da Fehl- 
zuordnungen zu ungenauen oder gar inkonsistenten Karten führen kön- 
nen. Als drittes Kriterium kann man die Erkennung oder Behandlung von 
dynamischen Objekten nennen, da diese nicht dauerhaft mit in die Karte 
aufgenommen und nicht für die Lokalisierung verwendet werden sollten. 

Das SLAM-Problem kann auch als Markov-Kette interpretiert und mit 
einem Bayes-Filter als rekursive Schätzung mit inkrementeller Kartierung 


gelöst werden. Der rekursive Korrekturschritt ist jedoch nicht geschlos- 
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sen lösbar, sodass approximative Verfahren zum Einsatz kommen. Wenn 
beispielsweise die Variablen, d.h. das Bewegungsmodell, das Messmodell 
und der SLAM-Posterior, als multivariate Gaußdichten modelliert werden, 
kann ein EKF zur Schätzung zum Einsatz kommen. Als wesentlich flexibler, 
was die Modellierung der Dichten und die Verfolgung mehrerer Datenzu- 


ordnungshypothesen angeht, haben sich Partikelfilter erwiesen. 


3.2.1 Partikelfilter 


Im Gegensatz zu Ansätzen mit Kalman-Filtern, welche eine parametri- 
sche Approximation vornehmen, wird beim Partikelfilter die Wahrschein- 
lichkeitsdichte mit Samples des Zustandsraums repräsentiert. Durch die 
Samples können multi-modale Dichten modelliert und implizit mehrere 
Hypothesen für die Datenzuordnung verfolgt werden. 

Ein Partikelfilter ist jedoch nur für Probleme mit wenigen Dimensio- 
nen geeignet, da die benötigte Anzahl von Partikeln im ungünstigsten Fall 
exponentiell mit der Dimension des Zustandsraum wachsen kann. Da die 
Dimension des Zustandsraums bei SLAM mit der Anzahl der Landmarken 
anwächst, wäre die einfache Anwendung eines Partikelfilters ungeeignet. 
Betrachtet man das bayessche Netz in Abbildung 3.3, so wird deutlich, dass 
unter der Bedingung eines bekannten Pfades die Landmarken voneinander 
unabhängig werden und in einzelne Schätzer faktorisiert werden kön- 
nen. Es ist daher möglich, das Partikelfilter durch Rao-Blackwellisierung 
geeignet umzuwandeln, indem anstatt nur der fortlaufenden Pose der 
gesamte Pfad geschätzt wird und jedes Partikel eine Pfadhypothese ver- 
folgt [Mur99]. 

Das resultierende Filter ist ein rao-blackwellisiertes Partikelfilter (RBPF) 
und eine der bekanntesten Implementierungen ist der in [Mon02] vorge- 
stellte FastSLAM-Algorithmus mit folgender Faktorisierung: 


p(x" Aki, uk ‚nk) = p(x ek, u t nt) [T pla’ z* uk Eat (3.36) 
i=1 


47 


3 Theoretische Grundlagen 


wobei nur noch die A-posteriori-Verteilung des Pfades durch die Partikel 
repräsentiert wird: 
v(x*|z,u*,n*) X (3.37) 


Jedes Partikel führt dann L unabhängige Landmarkenschätzer mit sich, 
d.h., auch jedes Partikel besitzt eine eigene Kartenhypothese. Unter der 
Annahme, dass die Beobachtungen gaußverteilt sind, können die Land- 
markenschätzer als individuelle EKFs realisiert werden. Werden mehrere 
Landmarken gleichzeitig beobachtet, können diese nacheinander aktuali- 
siert werden, da sie voneinander bedingt unabhängig sind. 

Es ist nicht möglich, die Partikel direkt aus der eigentlichen A-posteriori- 
Wahrscheinlichkeitsverteilung nach Gleichung (3.37) des Pfads zu ziehen, 
welche in der englischen Literatur als target distribution bezeichnet wird 
und sich mit Zielverteilung übersetzen lässt. Die rekursive Filterung kann 
jedoch, wie bereits angesprochen, in zwei Phasen aufgeteilt werden. Bei 
der in der englischen Literatur als sequential importance sampling (SIS) 
bezeichneten Methode wird in der ersten Phase jedes Partikel von einer 


als proposal distribution bezeichneten Verteilung gezogen [Che03]: 


Deh 


21 uk, ni bli) =p(xt! uz, all, 


pipe la 


mee eeh ` (3.38) 


welche aus der Propagation der Wahrscheinlichkeitsverteilung aus dem 


vorhergehenden Zeitschritt p (ar ER e 1) mit dem proba- 


bilistischen Bewegungsmodell p (xl lur, xl! i gebildet wird. Dabei wird 
eine zum i-ten Partikel zugehörige Variable mit hochgestelltem [i] gekenn- 
zeichnet. 

In der zweiten Phase wird jedes Partikel mit einem sogenannten impor- 
tance weight w I gewichtet. Dieses Gewicht entspricht dem Quotienten 
aus Zielverteilung (target distribution) und vorgeschlagener Verteilung 
(proposal distribution). Folglich wird der SLAM-Posterior durch die gewich- 
teten Partikel repräsentiert und die Gewichte wi haben die folgende 
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Form [Mon07]: 


k,[i]|l k yk ak. fi 
[i] _ target distribution _ p(x "ei. u“, n 1) 
WE proposal distribution d ( k,li]|zk-1 yk nk-1 1) 
D(x lize! uk, nk 
x plack tl, zt, wk stil, (3.39) 


d.h., das Gewicht ist proportional zum Likelihood der Beobachtung. Da es 
sich bei dem Schätzer für die Landmarken um einen EKF handelt, kann der 
Likelihood der Beobachtung und damit das Gewicht aus der Innovation, 
d.h. der Differenz der Beobachtung zg und der zugeordneten prädizierten 
Beobachtung 2, i g berechnet werden und hat für Punktlandmarken die 


folgende Form: 


See ls ul Sch a) 
w; œ —— exp|—-|Zk -Z ji Z, EIER ; 3.40 
ee le) rm 
wobei Z; die Innovationskovarianzmatrix ist: 
Zk = GL, 1 pr +R;. (3.41) 


Dabei ist Gr das linearisierte Messmodell, Lu LE die Kovarianz der zuge- 
ordneten Landmarke und Rx die Messunsicherheit [Mon07]. 

Wenn der rekursive Algorithmus lange Zeit ausgeführt wird, können 
die Gewichte der Partikel degenerieren, d.h., die meisten Gewichte w e 
werden sehr klein. Um dieser Degeneration entgegen zu wirken, wurde 
ein Algorithmus namens sequential importance resampling (SIR) entwickelt 
[Rub88]. Dabei werden nach der Gewichtung die Partikel aus den aktu- 
ellen Partikeln proportional zu wi) gezogen mit Zurücklegen. Danach 
werden alle Gewichte auf w I = 1/I gesetzt, wobei J die Anzahl der Par- 
tikel ist. Dies bedeutet, dass anschließend alle Partikel dasselbe Gewicht 
haben. Durch dieses sogenannte Resampling werden Partikel mit gerin- 


gem Gewicht, d.h. mit einer niedrigen Wahrscheinlichkeit, aussortiert, 
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während Partikel mit hohem Gewicht und damit auch hoher Wahrschein- 
lichkeit mehrfach gezogen werden. 

Das Resampling kann zwar effektiv die Degeneration verhindern, kann 
jedoch auf der anderen Seite zu einer Verminderung der Diversität füh- 
ren, d.h., dass eine ausreichende Abdeckung des Zustandsraums nicht 
mehr gegeben ist. In [Dou00b] wurde deshalb eine adaptive Resampling- 
Strategie eingeführt, welche das Resampling nur ausführt, wenn die Parti- 
kel die Zielverteilung nicht mehr gut approximieren, was der Fall ist, wenn 
die Varianz der Gewichte aller Partikel zu groß wird. Ein Maß für die Qua- 
lität der Approximation kann demnach durch die effektive Sampleanzahl 


geschätzt werden: 
N 1 
ass (3.42) 


2 
I 
ra (wt!) 


Nur wenn /, unter einen gewissen Schwellwert fällt, wird das Resampling 
ausgeführt. 

Eine Situation, in der meist sehr viele Partikel beim Resampling eli- 
miniert werden, ist das Schließen von Schleifen. Durch die Verwendung 
eines relativen Bewegungssensors wie Rad-Odometrie und die Kartierung 
neuer Gebiete entlang des Pfades in unbekannter Umgebung akkumulie- 
ren sich die Fehler. Wenn der mobile Roboter eine Schleife schließt, kehrt 
er in bekanntes Gebiet zurück und die Unsicherheit in der Pose verringert 
sich. Dadurch erhalten einige Partikel ein sehr hohes Gewicht, während 
viele Partikel ein niedriges Gewicht erhalten und im Resampling mit hoher 
Wahrscheinlichkeit aussortiert werden. D.h., ein Aspekt im Zusammen- 
hang mit dem Schließen von Schleifen ist die nötige Anzahl von Partikeln, 
um deren Diversität aufrecht zu erhalten. Je größer die Schleife ist, desto 
mehr Partikel werden benötigt, um den Zustandsraum adäquat abzude- 
cken [Mon07]. 
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Verbesserung der vorgeschlagenen Verteilung 


Partikelfilter liefern die besten Ergebnisse, wenn die vorgeschlagene Ver- 
teilung und die Zielverteilung ähnlich sind. Aus diesem Grund kann es 
nachteilig sein, die Partikel nur von dem Bewegungsmodell zu ziehen, falls 
die Bewegungssensoren des mobilen Roboters sehr viel ungenauer sind als 
der perzeptorische Umgebungssensor, da in diesem Fall viele Partikel ein 
geringes Gewicht erhalten und beim Resampling verworfen werden. Um 
eine vorgeschlagene Verteilung zu erhalten, welche näher an der Zielver- 
teilung liegt, sollten die Beobachtungen aus dem perzeptorischen Umge- 
bungssensor mit eingehen [Mon07]. 

Dies wurde mit FastSLAM 2.0 umgesetzt, bei dem die Partikel xl 
dann eine Pose proportional zu einer neuen vorgeschlagenen Verteilung 
DEN xt] zk uk, sti ziehen, welche nun auch die aktuelle Beob- 


achtung z; beinhaltet. Die neue vorgeschlagene Verteilung wird mittels 


eines EKF für jedes Partikel realisiert, wobei die Prädiktion mit dem Bewe- 
gungsmodell durchgeführt und die Messung zx mit dem Korrekturschritt 
integriert wird. 

Nach [Dur06a] lassen sich die Gewichte dann mit 


DEN 


xl u«)p(zux* li] H geL uf, still 


DEN stat, wk, nhl) 


DEG 


wi WH (3.43) 


berechnen. Dabei lässt sich der Nenner aus der verbesserten vor- 
ll, Li] 
k k-1? 
aus dem Bewegungsmodell und der Likelihood der Beobachtung 


p(zu [xt ski uk, stil berechnet sich analog zu Gleichung (3.40). 


Betrachtet man die Laufzeit des RBPF-SLAM, so steigt diese bei 
der Berechnung der vorgeschlagenen Verteilung, der Berechnung der 


geschlagenen Verteilung berechnen. p(x 


X 


ux) ergibt sich 


Gewichte und der Aktualisierung der Karte lediglich linear mit der Anzahl 
der Partikel: O(/). Außerdem sind diese Teile des Algorithmus einfach 


parallel zu berechnen, da die Partikel voneinander unabhängig sind. Die 
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Laufzeit des Tests, ob Resampling notwendig ist, steigt ebenfalls linear mit 
der Anzahl der Partikel, der Test lässt sich jedoch nicht parallel berechnen. 
Das Resampling selbst hat im schlechtesten Fall eine Laufzeit mit O(L - I), 
d.h., sie steigt mit der Anzahl der Partikel mal die Kartengröße, die Aus- 
führung ist jedoch bei adaptivem Resampling sehr selten notwendig. Die 
Berechnung der Lokalisierung in der Karte lässt sich wieder in einfacher 


Weise parallelisieren, hängt jedoch von der Kartenart ab. 


3.2.2 Landmarkenkarte 


Eine Landmarkenkarte besteht aus einzelnen markanten Objekten, wel- 
che durch ihre beobachtbaren Merkmale beschrieben sind. Für die präzise 
und korrekte Kartierung mit SLAM ist eine robuste Datenzuordnung zwi- 
schen den Beobachtungen und den bereits kartierten Landmarken uner- 
lässlich, weshalb sie auch als zweites Konvergenzkriterium geführt wird, 
vgl. Abschnitt 3.2. 

Wie bereits erwähnt, repräsentiert ein Partikelfilter mehrere Hypothe- 
sen der Datenzuordnung, da diese pro Partikel durchgeführt wird. Partikel 
mit falschen Datenzuordnungen werden geringere Gewichte erhalten und 
werden mit höherer Wahrscheinlichkeit in einem späteren Resampling- 
Schritt verworfen, was als verzögerte Entscheidungsfindung betrachtet 
werden kann. Obwohl dies ein großer Vorteil ist, können viele falsche 
Zuordnungen einen negativen Einfluss haben, da das Verwerfen vieler 
Partikel die Diversität verringert und damit indirekt das erste Konvergenz- 
kriterium beeinträchtigt [Mon07]. 

Werden die Merkmale der Landmarke als gaußverteilt modelliert, kann 
die Wahrscheinlichkeit der Beobachtung einer bereits in der Karte vorhan- 
denen Landmarke als Funktion der Innovation pro Partikel berechnet und 


damit am Beispiel einer Punktlandmarke zur Datenzuordnung verwendet 
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werden: 


oi =arg GC lil zk yk all, SEN (3.44) 


k 


1 1 k Tazi x 
= argmax —— exp| -3 (x A ) Zy (zx A H 
EE 2 ng ‚k ng ‚k 


[i] 
Wi 


(3.45) 


mit der Innovationskovarianzmatrix Z; entsprechend Gleichung (3.41). 
Dies ist eine Instanz des Maximum-Likelihood-Schätzers (ML-Schätzer) 
für multivariate Gaußdichten [Kro04]. Die ML-Datenzuordnung hat eine 
Laufzeit von schlechtestenfalls O(L - I), wenn alle Landmarken überprüft 
werden. Wenn der Likelihood für alle möglichen Landmarkenkandidaten 
unter einem gewissen Schwellwert liegt, wird eine neue Landmarke in 
der Karte instanziiert. Ist dieser Schwellwert zu hoch gewahlt, werden 
Landmarken falschlicherweise mehrfach in die Karte eingetragen. Ist er zu 
niedrig gewählt, werden neu beobachtete Landmarken falschlicherweise 
bereits bestehenden zugeordnet. 

In den meisten Veröffentlichungen über SLAM mit Landmarkenkarten 
werden die Landmarken als Punktlandmarken modelliert, d.h., dass sie 
lediglich über ihren Ort definiert sind. In Abschnitt 4.1 wird zur Behand- 
lung dieser Problematik ein erweitertes Landmarkenmodell für eine robus- 


tere Datenzuordnung vorgestellt. 


3.2.3 2D-Rasterkarte 


Eine 2D-Rasterkarte mit Belegtheitsinformation (englisch: occupancy grid 
map) kann sowohl Informationen über den belegten Raum als auch über 
freie bzw. befahrbare Bereiche repräsentieren. Jede Zelle der Karte spei- 
chert dafür ihre Belegtheitswahrscheinlichkeit. Diese geht von 0, was 
sicher frei bedeutet, über unbekannt bei 0,5 bis zu 1, was sicher belegt 
bedeutet [Thr05]. Die 2D-Rasterkarte ist eine dichte Umgebungsreprä- 
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sentation ohne Annahmen über die Struktur der Umgebung, d. h., sie ist 
sowohl für strukturierte als auch unstrukturierte Umgebungen geeignet. 

Bei der Umgebungsrepräsentation in Form einer 2D-Rasterkarte wird 
das Problem der Kartenschätzung in separate Schätzer je Zelle aufgeteilt, 
d.h., die A-posteriori-Schätzung über die Karte wird in das Produkt der 
einzelnen Schätzer faktorisiert: 


p(Alz*,x*) = i DE ll R (3.46) 


mit A als Karte und A; als /-te Zelle. Durch diese Aufteilung kann die Belegt- 

heitswahrscheinlichkeit sehr effizient geschätzt werden. Abhängigkeiten 

zwischen benachbarten Zellen können jedoch nicht modelliert werden. 
Für die Berechnung der Belegtheitswahrscheinlichkeiten der Zellen 


wird die sogenannte Logit-Form verwendet: 


dea 


EEN . (3.47) 


Lei = log 


Der Vorteil der Logit-Form ist, dass sie keine numerischen Instabilitäten 
wie die Wahrscheinlichkeitsdarstellung bei Multiplikationen nahe 0,0 und 
1,0 aufweist. Die entsprechende Wahrscheinlichkeit kann aus der Logit- 


Form in einfacher Weise wieder rekonstruiert werden: 


p(Aılz*,x*) sie ea (3.48) 


Die Aktualisierung mit einer neuen Beobachtung wird rekursiv mittels 
Bayes-Filter durchgeführt: 


p\Aılzr,&k a 
Lk = Lk-1,1 + log | | — log pu) (3.49) 


1 - p(aifex, xx) 1-p(A) 
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wobei p(Aı) die A-priori-Belegtheitswahrscheinlichkeit ist. Ein weiterer 
Vorteil der Logit-Form ist hier, dass die Multiplikation bei der Aktuali- 
sierung durch eine Addition ersetzt wird, was wesentlich effizienter zu 
berechnen ist [Thr05]. 

Die Lokalisierung in einer 2D-Rasterkarte kann über Scan-Matching 
mit einem 2D-LiDAR-Scan erfolgen. Dieses Scan-Matching kann beispiels- 
weise mittels Bergsteigeralgorithmus (in der englischen Literatur als hill- 
climbing bekannt) durchgeführt werden [Sic08; Rus09]. Dabei wird die 
Zielfunktion aus den kleinsten Entfernungen der Strahlenendpunkte des 
2D-LiDAR-Scans zur jeweils nächsten belegten Zelle berechnet [Thr05]. 

In [Gri07] wurde ein SLAM-Verfahren vorgestellt, das eine 2D-Ras- 
terkartierung mit Belegtheitswahrscheinlichkeiten mit dem FastSLAM-2.0- 
Algorithmus anhand von 2D-LiDAR-Daten und Rad-Odometrie durchführt. 
Die Verwendung von FastSLAM 2.0 bedeutet, dass die Information des 
Umgebungssensors bereits in die vorgeschlagene Verteilung mit einbezo- 
gen wird. Sie ist im Falle eines 2D-LiDARs i.d.R. sehr viel genauer als 
die Bewegungsschätzung lediglich aus der Rad-Odometrie und die vorge- 
schlagene Verteilung wird somit auch hier ähnlicher zur Zielverteilung. 
Nach der Propagation der Partikel aus dem vorherigen Zeitschritt anhand 
der Odometriedaten wird zunächst für jedes Partikel ein Scan-Matching 
wie oben beschrieben durchgeführt. Anschließend wird für die Lokalisie- 


rung jeweils eine Normalverteilung N (xt ; Si als vorgeschlagene Ver- 


teilung geschätzt, indem die Zielfunktion DO IA n pX j) zusammen mit 
[i] 

k-1? 
Registrierung gefundenen Pose an J Stellen abgetastet wird: 


dem Bewegungsmodell p(x; x o in einem Intervall um die bei der 
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sl =- Ye sl NI ob (3.50) 
FS 
Ht =y oles ws) 8)" 
y= 
(3.51) 


J 
au = >" plat tj) (xs bef, o) (3.52) 
Die Gewichte der Partikel lassen sich dann wie folgt berechnen: 


wy ec pls), scil mer) = «ll. Ga 
JA 


In Abschnitt 4.2 wird ein alternativer Ansatz für die Berechnung 
der vorgeschlagenen Verteilung auf Basis eines EKF vorgestellt, welcher 
einen modularen Aufbau zur flexiblen Kombination mit anderen Kar- 
tenrepräsentationen und zusätzlichen Lokalisierungssensoren bietet. Mit 
diesem Ansatz wird in Abschnitt 4.3 ein hybrider SLAM-Algorithmus mit 
einer Kombination aus dichter Karte und Landmarkenkarte vorgestellt. 
In Abschnitt 5.1 erfolgt die Vorstellung der Integration eines zusätzlichen 


absoluten Lokalisierungssensors. 


3.2.4 3D-NDT-Karte 


Für die 3D-Kartierung ist eine speichereffiziente Datenstruktur wünschens- 


wert, und es ist von Vorteil, wenn der Speicherbedarf nicht mit der Anzahl 
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der aufgenommenen Punkte bzw. Kartierungsdauer, sondern lediglich von 
der Kartengröße und deren Auflösung abhängt. 

Eine 3D-NDT-Karte erlaubt eine kompakte Repräsentation der Umge- 
bung und es müssen nicht alle Daten der 3D-Punktwolken gespeichert 
werden. Dennoch wird bei NDT-Karten im Vergleich zu reinen Belegt- 
heitskarten eine Schätzung über die ursprüngliche Verteilung der Punkte 
und damit eine feiner aufgelöste Repräsentation der Umgebung bei glei- 
cher Auflösung erzielt. D. h., dass eine NDT-Karte bei gleicher Auflösung 
expressiver ist als eine reine Belegtheitskarte und dabei robuster gegenüber 
verrauschten Sensordaten ist [Saa13]. 

Bei einer 3D-NDT-Karte wird die Umgebung in ein 3D-Raster aus 
Voxeln aufgeteilt und für jedes Voxel wird anhand der darin liegenden 
Punkten eine Normalverteilung geschätzt, d.h., die Punkte werden in 
Normalverteilungen transformiert. Die Normalverteilung des /-ten Voxels 
N (a, Da) wird für die M Punkte p mit der korrigierten Stichprobenkova- 
rianz folgendermaßen geschätzt [Mag09]: 


1 M 
it; = — ak 3.54 
i= ag LP (3.54) 
1 M 
Il, = m — RI) (Pm — A). 3.55 
i m LP #1) (Pm = ĉi) (3.55) 


Abbildung 3.4 zeigt eine Visualisierung unterschiedlicher 3D-Normalver- 
teilungen als 30-Ellipsoide. In Abbildung 3.4a sind die Punkte gleichver- 
teilt, d. h., alle Eigenwerte der entsprechenden Kovarianzmatrix sind zirka 
gleich groß: ex = e, = €,. Abbildung 3.4b entspricht einer länglichen 
Struktur, d.h., ein Eigenwert ist sehr viel größer als die beiden anderen: 
Ex X €y X €z. Abbildung 3.4c entspricht einer flachen horizontalen Fläche, 
wie z.B. einer Straßenoberfläche, d.h., ein Eigenwert ist sehr viel kleiner 
als die beiden anderen: ex ~ €y > €z. 

Mit den Gleichungen (3.54) und (3.55) ist keine effiziente inkrementelle 


Aktualisierung möglich. Außerdem müssten alle Punkte gespeichert wer- 
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()xzeyre& (b) ex see e: (c) ex seu > & 


Abbildung 3.4: Beispiele für NDT-Ellipsoide. 


den, was der Forderung nach einer speichereffizienten Datenstruktur ent- 
gegen steht. Daher wurde in [Tak06] eine Methode für eine inkrementelle 
Aktualisierung vorgestellt, welche allerdings im Gegensatz zu Gleichung 
(3.55) auf einer nicht-korrigierten Kovarianzschätzung beruht. 

In dynamischen Umgebungen, d.h. in Umgebungen mit sich bewegen- 
den Objekten, ist es von Vorteil, auch Information der Messungen über den 
freien Raum mit einzubeziehen, wie es auch bei der 2D-Belegtheitskarte der 
Fall ist. Dadurch können dynamische Objekte aus der Karte entfernt wer- 
den und zusätzlich auch durch Ungenauigkeiten fälschlicherweise kartierte 
Bereiche wieder als frei markiert werden. In [Saa13] wurde ein NDT-OM 
genanntes Verfahren vorgestellt, welches ebenfalls eine Belegtheitswahr- 
scheinlichkeit mit der Logit-Form wie bei der im vorherigen Abschnitt vor- 
gestellten 2D-Belegtheitskarte einbezieht. Hierbei wird ein heuristisches 
Verfahren eingesetzt, welches die Wahrscheinlichkeit jedes Voxels auf dem 
Weg des LiDAR-Strahls verringert, bis dieser an einem Objekt reflektiert 
wird, d.h. auf ein Hindernis trifft. In der Zelle, in der der Strahl auf ein 
Hindernis trifft, wird die Wahrscheinlichkeit für die Belegtheit erhöht. Ein 
wichtiger Unterschied zu einer reinen Belegtheitskarte ergibt sich daraus, 
dass bei der NDT-Karte in dem Voxel eine zusätzliche räumliche Struktur 
modelliert ist, wohingegen das Volumen des Voxels bei einer Belegtheits- 


karte als homogen angenommen wird. Da die räumliche Struktur nur einen 
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Teil des Voxels belegt, tragen durch das Voxel hindurchgehende Strahlen 
weniger negative Information, als dies bei einer reinen Belegtheitskarte der 
Fall wäre, und die Verringerung der Belegtheitswahrscheinlichkeit erfolgt 
entsprechend konservativer. 

In Abschnitt 5.3.2 wird eine performante Implementierung mit inkre- 
menteller Aktualisierung auf Basis einer korrigierten Kovarianzschätzung 
und einer zusätzlichen Existenzwahrscheinlichkeit für die Berücksich- 
tigung negativer Information mittels einer effizienten Umsetzung eines 


Strahlverfolgungsalgorithmus vorgestellt. 
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Erweiterte SLAM-Verfahren 


In diesem Kapitel werden Erweiterungen vorgestellt, welche eine robus- 
tere Kartierung erlauben. So kann durch die Modellierung von Landmar- 
ken mit zusätzlichen Merkmalen die Zuverlässigkeit der Datenzuordnung 
zwischen Beobachtungen und bereits kartierten Landmarken verbessert 
werden. Des Weiteren können durch die Kombination von dichten Karten 
mit landmarkenbasierten Karten die Robustheit und Präzision in heteroge- 


nen Umgebungen erhöht werden. 


4.1 Kartierung mit erweitertem 
Landmarkenmodell 


Die robuste Datenzuordnung beobachteter Landmarken zu bereits kar- 
tierten Landmarken ist eine wichtige Voraussetzung für die Erzeugung 
konsistenter und präziser Karten und das zweite Konvergenzkriterium für 
SLAM-Algorithmen, siehe Abschnitt 3.2. So können falsche Zuordnungen 
dazu führen, dass das SLAM-Verfahren divergiert und die erzeugte Karte 
ungenau wird. Werden sehr viele Beobachtungen falsch zugeordnet, kann 


dies das Erzeugen einer konsistenten Karte sogar unmöglich machen. Eine 
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Modellierung der Landmarken durch zusätzliche Merkmale, welche eine 
Unterscheidung distinktiver machen, soll eine zuverlässigere Zuordnung 
erlauben. Dies führt im Fall von ansonsten vereinzelt aufgetretenen Fehl- 
zuordnungen zu einer präziseren Karte. Sind sehr viele Beobachtungen 
ohne die zusätzlichen Merkmale nicht robust zuzuordnen, so kann die 


Erweiterungen eine korrekte Kartierung überhaupt erst ermöglichen. 


4.1.1 Datenzuordnung 


In vielen EKF-basierten SLAM-Algorithmen und den ursprünglichen 
FastSLAM- und FastSLAM-2.0-Algorithmen sind Landmarken als 2D- 
Punktobjekte modelliert, d. h., sie sind lediglich über ihre zwei Koordinaten 
definiert. Dies macht die Datenzuordnung anfällig für Mehrdeutigkeiten 
in Regionen, in denen die Landmarken dicht beieinander liegen und vor 
allem beim Schließen von Schleifen, da hier die Bewegungsunsicherheit 
i. A. am größten ist. Letzteres ist der Fall, weil die Unsicherheit des mobilen 
Roboters bei der Fahrt durch unbekanntes Gebiet anwächst und erst nach 
dem Schließen der Schleife durch die Beobachtung bekannter Landmarken 
reduziert werden kann. 

Werden Landmarken lediglich über ihre Position modelliert, gibt es 
zwei Ursachen, welche die Zuverlässigkeit beeinflussen. Diese sind die 
in Abbildung 4.1 dargestellte Bewegungsunsicherheit und die Messunsi- 
cherheit. Während die Messunsicherheit zu einzelnen Fehlzuordnungen 
führen kann, kommt es durch die Bewegungsunsicherheit möglicherweise 
zu mehreren Fehlzuordnungen gleichzeitig. Zudem ist die Bewegungsun- 
sicherheit aufgrund der vergleichsweise ungenauen Rad-Odometrie meist 
größer als die Messunsicherheit, wie in Abschnitt 3.2.1 erläutert. 

Da die Schwierigkeit der Unterscheidung vor allem damit zusammen- 
hängt, dass die Position das einzige Merkmal ist, liegt es nahe, dem Modell 
der Landmarke weitere vom Ort unabhängige, also z.B. ansichtsbasierte, 
Merkmale hinzuzufügen, um ihre Unterscheidbarkeit zu verbessern. Dies 


sollte vor allem in Situationen, in denen die Landmarken dicht beisammen 
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Abbildung 4.1: Mehrdeutigkeit durch Messunsicherheit links und Mehrdeutigkeit durch 
Lokalisierungsunsicherheit rechts, nach [Mon07]. A: Mobiler Roboter, ©: Landmarke, @: 
Messunsicherheit, ©: Lokalisierungsunsicherheit. 


sind oder die Unsicherheit in der Pose des Roboters besonders groß ist, die 
Datenzuordnung verbessern [Emt10a]. 

Als zugrundeliegender SLAM-Algorithmus wurde ein rao-blackwel- 
lisiertes Partikelfilter (RBPF) gewählt, da dieses, wie in Abschnitt 2.2.1 
dargestellt, gegenüber den auf dem Kalman-Filter basierenden Methoden 
einige Vorteile bietet. So können bei EKF-basierten Verfahren lediglich 
gaußverteilte Merkmale direkt integriert werden, während die bedingten 
Unabhängigkeiten beim RBPF eine flexiblere Gestaltung des Landmarken- 
modells erlauben. 

Da als SLAM-Verfahren ein Partikelfilter zum Einsatz kommt, sollen 
die zusätzlichen Merkmale nicht nur für die Zuordnung verwendet wer- 
den, sondern auch in die Berechnung der Gewichte der Partikel einfließen. 
Dies hat darüber hinaus den Vorteil, dass Partikel, welche dennoch falsch 
zugeordnete Beobachtungen integriert haben, niedrigere Gewichte besit- 
zen und damit bei einem späteren Resampling wahrscheinlicher eliminiert 


werden. 


4.1.2 Erweitertes Landmarkenmodell 


Für eine robuste Datenzuordnung wurde ein erweitertes Landmarkenmo- 


dell entworfen, welches vertikale zylindrische Objekte, wie Baumstämme 
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und Laternenpfähle, abbildet. Zusätzlich zu der Position enthält es eine 
visuelle Signatur als ansichtsbasiertes Merkmal und den Radius der Land- 
marke. Durch den Radius wird die horizontale Ausdehnung einer Land- 
marke beschrieben. Idealerweise sind die Signatur und der Radius orts- 
unabhängig, sodass sie auch bei großer Lokalisierungsunsicherheit des 
mobilen Roboters eine zuverlässigere Zuordnung erlauben. 

Das erweiterte Landmarkenmodell A beschreibt ein vertikales zylin- 
drisches Objekt mit dem Merkmalsvektor A = [xa, ya, ra, v], welcher 
sich aus geometrischen Merkmalen m = [xı,ya,rı] für (x-Koordinate, 
y-Koordinate und Radius) und einer visuellen Signatur v zusammensetzt. 
Dabei entsprechen die x- und y-Komponenten in Kartenkoordinaten dem 
einfachen Punktlandmarkenmodell. Die beiden anderen Merkmale wer- 
den als unabhängig vom Ort angenommen und erweitern das einfache 
Punkt-Landmarkenmodell. 

Zur Extraktion der Landmarken wird ein hierarchisches Extraktions- 
verfahren basierend auf 2D-LiDAR- und Kamerabilddaten angewendet. Die 
Kombination dieser beiden Sensoren hat den Vorteil, dass die hohe Ent- 
fernungsgenauigkeit des LiDARs mit der höheren Winkelauflösung der 
Kamera verbunden wird. Die beiden Sensoren werden aufeinander kali- 
briert, sodass die LiDAR-Punkte in das Kamerabild projiziert werden kön- 
nen, in Abbildung 4.2 als farbige Punkte dargestellt. Die weitaus höhere 
Auflösung der Kamera wird anhand des Vergleichs der Pixelgröße zu dem 
weiten horizontalen Abstand der Punkte des LiDAR-Scans deutlich [Ulr10]. 


Detektion 


Zur Detektion der Landmarken wird eine hierarchische Strategie mit den 
Daten aus beiden Sensoren angewendet. Bei der Detektion werden zuerst 
die LiDAR-Daten betrachtet, da sich die Landmarken in dem Scan deutlich 
als lokale Entfernungsminima abzeichnen. In Abbildung 4.2 sind die Punkte 
der Landmarkenkandidaten durch größere farbige Quadrate gekennzeich- 
net. Da im Scan auch einige andere Objekte, wie z. B. Steine oder Büsche, als 
Kandidaten bei er Auswertung der LiDAR-Daten erkannt werden, wird im 
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Abbildung 4.2: Ein in das Kamerabild referenzierter Scan des 2D-LiDARs (farbige Punkte). 
Größer hervorgehobene Punkte zeigen mögliche Landmarkenkandidaten nach dem ersten 
Detektionsschritt. Das grüne Rechteck bezeichnet eine nach dem zweiten Schritt verifizierte 
Landmarke und den Ausschnitt, welcher zur Berechnung der visuellen Signatur verwendet 
wird [Ulr10]. 


Kamerabild zusätzlich nach vertikalen Kanten gesucht, um solche Fehlde- 
tektionen auszuschließen bzw. schmale vertikale Strukturen zu verifizieren. 
Dafür werden vertikale Kanten links und rechts von dem Kandidaten in 
dem Kamerabild gesucht. Nur Kandidaten, welche zwei deutliche vertikale 
Kanten aufweisen, entsprechen dem Landmarkenmodell und werden als 
Detektion gewertet. In Abbildung 4.2 zeigt das grüne Rechteck eine final 
verifizierte Landmarkendetektion, wobei die horizontale Ausdehnung des 
Rechtecks durch die linke und die rechte aus dem Kamerabild extrahierte 
Kanten ermittelt wurde. Dieses dient als Segmentfläche für die Bestim- 
mung der visuellen Signatur [Ulr10]. 


Merkmalsbestimmung 


Nach der Detektion werden die Messdaten der Landmarke ermittelt. Der 
horizontale Richtungswinkel (Azimut) a, und die Entfernung zur Oberflä- 
che der Landmarke dr werden zunächst aus den Messungen des LiDARs 
bestimmt, da dessen Entfernungsmessungen eine hohe Genauigkeit auf- 
weisen. Der Radius der Landmarke r4 wird wegen der höheren Winkelauf- 
lösung aus den Bilddaten anhand der vom LiDAR ermittelten Entfernung 
dr. vom LiDAR bestimmt, welche auch der Entfernung von der Kamera 
entspricht: 


D 
rı = din (4.1) 
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wobei c für die Kamerakonstante der Kamera in horizontaler Richtung des 
Sensors steht und ò für die bei der Detektion ermittelte Distanz zwischen 
der rechten und der linken Kante der Landmarke in Pixeln im Bild. Da dh 
der Entfernung zur Oberfläche der Landmarke entspricht, ergibt sich durch 
Addition des Radius r4 auf die Entfernung d, die beobachtete Entfernung 
des Zentrums der Landmarke dy [Ulr10]. Zusammen mit dem Azimut a, 
und dem Radius ra ergeben sich die drei geometrischen Komponenten der 
Beobachtung der Landmarke: 


dı +r, dı 
Zm,k = ar = laıl. (4.2) 
ra ra 


Die visuelle Signatur der Landmarke v besteht aus einer Schätzung der 
Farbverteilung, welche über die Segmentfläche innerhalb des detektierten 
Rechtecks aus den Bilddaten bestimmt wird. Die Farbinformationen wer- 
den über die jeweilige Segmentfläche berücksichtigt. Als Farbmodell wird 
das in [Com00] vorgeschlagene HSV-Farbmodell verwendet. HSV steht im 
Englischen für hue - Farbwert, saturation - Sättigung und value - Hell- 
wert oder Helligkeit [Smi78]. Der HSV-Farbraum wurde gewählt, da dieser 
im Vergleich zum RGB-Farbraum die Farbinformation von der Helligkeit 
entkoppelt und robuster gegenüber Veränderungen in der Umgebungs- 
helligkeit ist. Damit ist die visuelle Signatur unabhängiger von lokalen 
Abschattungen. 

Für die Schätzung der Farbverteilung werden zunächst Histogramme 
über die Kanäle mit den Klassen yy, ys und yy gebildet. Dabei werden 
für yy und re nur Pixel verwendet, deren Farbwert bzw. Sättigung über 
einem jeweiligen Schwellwert liegen, um hauptsächlich die Farbinforma- 
tion zu berücksichtigen. Die übrigen Pixel werden dann für die Bildung 
von yy verwendet, da auch die nahezu farblosen Bereiche Informationen 
für die Wiedererkennung liefern [Pér02]. Jedes Histogramm beinhaltet B 
Klassen und bei der Berechnung der Farbverteilung werden die Klassen zu 


einem Gesamthistogramm y, = Tue + yy zusammengefasst. Der Vektor 


66 


4.1 Kartierung mit erweitertem Landmarkenmodell 


é beinhaltet die Histogrammklassen der Segmentflache mit U Pixeln und 
&(u) € 1,..., B bezeichnet die Histogrammklasse an der Pixelstelle u, kor- 
respondierend mit dem Pixel im HSV-Farbraum. Die geschätzte Dichte der 


beobachteten Farbverteilung wird dann nach [Pér02] über 


U 
2,(b)=n ) ,ö(&(u) - b) (4.3) 
u=1 
für alle b € 1,..., B bestimmt. Dabei ist 6 die Dirac-Funktion 
1, v=0 
ô = 4.4 
(v) | 0, sonst 


und n ein Normalisierungsfaktor, sodass EE z,(b) = 1 ist. Damit ent- 
spricht z, einer diskretisierten Dichte und kann als visuelle Signatur der 
Landmarke dienen [Ulr10; Emt11]. 


Wiederkennung 


Für die Wiedererkennung einer bereits in der Karte existierenden Land- 
marke und für die Berechnung der Gewichte der Partikel für den zugrunde- 
liegenden Partikelfilter-SLAM-Algorithmus wird ein statistisches Distanz- 
maß benötigt. Hierfür bietet sich der häufig verwendete Likelihood an. Mit 
diesem kann sowohl mittels der Maximum-Likelihood-Methode die wahr- 
scheinlichste Zuordnung einer Beobachtung zu einer bereits kartierten 
Landmarke ermittelt werden als auch nach der Zuordnung das jeweilige 
Gewicht berechnet werden. Die statistische Unabhängigkeit der visuellen 
Signatur v von der Pose des Roboters ist dabei ein wichtiges Kriterium, 
denn nach dem Multiplikationssatz können individuell berechnete Like- 
lihoods verknüpft werden, wenn es sich um voneinander unabhängige 
Merkmale handelt. Der Likelihood der Beobachtung der geometrischen 
Merkmale zy, ist demnach unabhängig von dem Likelihood der Beob- 


achtung der visuellen Signatur z,,x. Daraus folgt, dass der Likelihood der 
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gesamten Beobachtung z, x aufgeteilt werden kann: 


plz. ski. uk, SE 


EE e 
SS pl ; (4.5) 
D.h., der Gesamt-Likelihood wird durch die Multiplikation des Likelihood 
bezüglich der geometrischen Beobachtungen Zm und dem Likelihood 
beziiglich der Beobachtung der visuellen Signatur z, berechnet. Wah- 
rend die geometrischen Beobachtungen als normalverteilt angenommen 
werden, wird die Beobachtung der visuellen Signatur, wie im vorherigen 
Abschnitt beschrieben, durch ein Histogramm modelliert. Die visuelle 
Signatur ist zudem von der Pose und der Bewegung des Roboters unab- 
hangig. 
Für die Berechnung des Likelihoods der Beobachtung der geometri- 
schen Merkmale wird zunächst für das i-te Partikel die prädizierte Beob- 


achtung $. o anhand der zugeordneten Landmarke und der geschätzten 
k 


Roboterpose aE = [xl $ lihi li]]T berechnet: 
d 
Salt = |atan2(dy, dx) ZU, (4.6) 
WË 
mit 

d =k An = zul, (4.7) 
dy =$, „u = gH, (4.8) 
de 4| d2 + d, (4.9) 
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wobei atan2 eine Erweiterung des Arkustangens ist, welche alle vier Qua- 
dranten abdeckt. 

Das zugehörige linearisierte Messmodell Gx ist die Jacobi-Matrix 
bezüglich der Beobachtung: 


0 
2 oi. (4.10) 
1 


Der Likelihood der Beobachtungen der geometrischen Merkmale 
wird anschließend mittels der Differenz zwischen der Beobachtung Zm,k 
nach Gleichung (4.2) und der prädizierten Beobachtung ZN nach Glei- 
chung (4.6) berechnet: 


D = I exp Bd k-Z m) Sch k-Z m) 
m,k (27)3/2 Ka SC WW? Ee mag > 
(4.11) 
mit der Innovationskovarianzmatrix 
Zk = GL, 16% +R;, (4.12) 


welche sich aus dem linearisierten Messmodell Gr nach Gleichung (4.10), 


der Kovarianzmatrix der in der Karte zugeordneten Landmarke Lt 


H ko 


und der Messfehlerkovarianz Rx zusammensetzt [Mon07]. 

Der Likelihood des Histogrammvergleichs zwischen der Beobachtung 
Zy,k und der geschätzten visuellen Signatur der zugeordneten Landmarke 
Wi kann wie folgt berechnet werden [Com00]: 


Prk = exp(-£Dy (zur, 2) (4.13) 


> 
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Dabei ist 


B 
KICHE H NT) (4.14) 


die Distanz zwischen den Histogrammen, wobei b die jeweilige Histo- 
grammklasse bezeichnet und B die Gesamtzahl der Klassen. Diese Form 
der Histogrammdistanz D, wurde von [Com00] zum Histogrammvergleich 
herangezogen und beinhaltet den Bhattacharyya-Koeffizienten 


B 
EK (4.15) 
b=1 


Es wurde weiterhin gezeigt, dass die Distanz D, einer Metrik entspricht, 
wohingegen die Bhattacharyya-Distanz Dg = -In(ß) selbst und die oft 
verwendete Kullback-Leibler-Divergenz mindestens eines der Axiome ver- 
letzen [Pér02]. So wird z.B. in [Cov91] angemerkt, dass die Kullback- 
Leibler-Divergenz zwar oft als Distanz zwischen zwei Wahrscheinlich- 
keitsdichten angegeben wird, sie jedoch die Dreiecksungleichung nicht 
erfüllt. Die Bhattacharyya-Distanz erfüllt ebenso nicht die Dreiecksunglei- 
chung [Bha09]. 

Die letztendliche Datenzuordnung wird pro Partikel mit dem Maximum- 
Likelihood-Schätzer durchgeführt: 

ol = arg max aleet, gt. ur, nl! d CHEN 
ali] 
= arg max pet pl ; (4.16) 


[i] 
nk 


mit De , für die geometrischen Merkmale nach Gleichung (4.11) und Dee 
fiir die visuelle Signatur nach Gleichung (4.13). Somit kann der Gesamt- 
Likelihood fiir die Datenzuordnung einer Beobachtung zu einer bereits 
kartierten Landmarke direkt berechnet werden. 
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Verbesserung der vorgeschlagenen Verteilung 


Bei dem Partikelfilter wird angelehnt an den FastSLAM-2.0-Algorithmus 
eine Verbesserung der vorgeschlagenen Verteilung durch die Beobachtun- 
gen bekannter und damit bereits kartierter Landmarken durchgeführt. 
Zunächst wird aus der zugeordneten Landmarke und der geschätz- 
ten Roboterpose des i-ten Partikels die prädizierte Beobachtung Zen al 
nach Gleichung (4.6) berechnet. Für die weitere Berechnung wird zusätz- 
lich wieder die Innovationskovarianzmatrix Z; nach Gleichung (4.12) und 
außerdem die Jacobi-Matrix bezüglich der Pose des Roboters benötigt: 


J=|å |Ë -å -1|. (4.17) 
0 


Daran, dass hier in der letzten Zeile alle Elemente 0 sind, ist zu erkennen, 
dass der Radius der Landmarke unabhangig von der Pose des Roboters 
angenommen wird. 
Das Residuum yx berechnet sich aus der Differenz der Messung und 
der prädizierten Messung der in der Karte zugeordneten Landmarke: 
Vk = Zm,k — 2 [i] (4.18) 


m,n, 


Anschließend kann die neue Systemkovarianzmatrix Be des i-ten 


Partikels berechnet werden: 


-1 1 
Ein = er Jk + |e TA | l (4.19) 
Die verbesserte Posenschätzung wird schließlich wie folgt berechnet: 


a= Sc ‚+ PAI Zi (4.20) 
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Hierbei ist zu beachten, dass es mehrere Korrekturschritte pro Zeitschritt 
k geben kann, wenn mehrere Landmarken gleichzeitig beobachtet werden. 
Ist dies der Fall, werden die Berechnungen von Gleichung (4.6) bis (4.20) 
für jede Landmarke nacheinander ausgeführt und verbessern so inkremen- 
tell die vorgeschlagene Verteilung. Da die visuelle Signatur unabhängig 
vom Ort ist, kann diese auch nichts zur Verbesserung der vorgeschlagenen 


Verteilung beitragen. 


Berechnung der Partikelgewichte 


Anschließend können die jeweiligen Gewichte der Partikel aktualisiert 
werden. Das Gewicht des i-ten Partikels wird nach Gleichung (3.43) mit 


den erweiterten Likelihoods pl? und pil wie folgt berechnet: 


[i] Li] 
p(x! Xj, ur) . . 
% See Phi, (421) 
p(x! xk-Llil zk, uk, nk (1) 
wobeies sich bei p (xt! xl! 2 ux) wieder um das Bewegungsmodell handelt 


und sich p(x!” [gti [il zb uk ak u) aus der verbesserten vorgeschlage- 


nen Verteilung mit den Gleichungen (4.19) und (4.20) berechnen lässt. 


Aktualisierung der Landmarken 


Die bisher in der Karte aufgezeichneten Landmarken werden nach der 
Zuordnung mit den jeweils zugeordneten Beobachtungen aktualisiert, 
indem die neuen Informationen integriert werden. Wie bereits dargestellt, 
werden die geometrischen Merkmale als gaußverteilt angenommen und 


deren Schätzung m kann somit in einem EKF-Korrekturschritt aktualisiert 
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werden: 
Vx = Zm,k ` lo, a) = Zm,k — Zant ; (4.22) 
Sk = GL, Dr +R;, (4.23) 
SH d Te-l 
Kx = L, ¢1 Ge Sk d (4.24) 
Tt g = WÄITE + Kıyk 5 (4.25) 
LA: = (I- KG Lu EA _ KG)" + KıRıK, S (4.26) 


wobei sich die prädizierte Messung ZN und das zugehörige linearisierte 
Messmodell G wieder aus Gleichungen (4.6) bzw. (4.10) berechnet. 

Die Schätzung der visuellen Signatur v ist durch eine diskrete Dichte 
bzw. ein Histogramm repräsentiert und kann mittels eines diskreten Bayes- 
Filters aktualisiert werden [Thr05]. Für alle Klassen b; des Histogramms 


wird eine Prädiktion durchgeführt: 


Fal ge- (Ps) = > P (osha: HLH a (4.27) 


D 


p(b ‚ur. bi) ist dabei die Übergangswahrscheinlichkeit der Klasse b; zu 
Klasse b;. Weil die visuelle Signatur von der Position des Roboters und 
damit auch von der Bewegung u; unabhängig ist, gilt DAD bi) =0 
für alle b; + b;. Daraus folgt, dass sich die Prädiktion zu Ŷ e — 
nl k|k-1 

Yall kie vereinfacht. 

Beim Korrekturschritt werden die jeweiligen Histogrammklassen b der 
Landmarke mit denen der Beobachtung z, (b) multipliziert: 


Pn kD) = NZy k (b)? nli Au P) > (4.28) 


wobei ņ ein Normalisierungsfaktor ist, mit dem das Histogramm normali- 
siert wird, sodass wieder Sra Vee (b) = 1 gilt [Ulr10; Emt10c]. 
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4.1.3 Zusammenfassung 


Bei dem vorgestellten Verfahren wird ein erweitertes Landmarkenmodell 
dazu verwendet, die Wiedererkennung durch zusätzliche ortsunabhän- 
gige und visuelle Merkmale robuster zu machen. Bei dem zugrundelie- 
genden SLAM-Algorithmus handelt es sich um ein RBPF inklusive der 
Verbesserung der vorgeschlagenen Verteilung. Während in [Dur06b] eine 
ansichtsbasierte Signatur lediglich zur Erkennung eines Schleifenschlusses 
dient, werden hier die zusätzlichen Merkmale in probabilistisch konsisten- 
ter Weise in die Schätzung des SLAM-Posteriors integriert. 

In [Ram07] wurde ein Verfahren vorgestellt, welches ebenfalls ein 
erweitertes Landmarkenmodell mit visueller Signatur zur robusteren Wie- 
dererkennung verwendet. Im Gegensatz zu dem hier vorgestellten Ver- 
fahren, welches auf einem RBPF basiert, wurde dort ein EKF als Schätzer 
verwendet, welcher gegenüber dem Partikelfilter einige Nachteile aufweist. 
So können nur multivariate Gaußdichten für die Merkmalsmodellierung 
verwendet werden, wie in Abschnitt 2.2.1 erläutert. Bei dem hier vorge- 
stellten SLAM-Algorithmus kann durch die Verwendung des Partikelfilters 
eine wesentlich flexiblere Modellierung der Landmarkenmerkmale erfol- 
gen. Durch die angenommene Unabhängigkeit der visuellen Signatur kann 
diese mit dem Multiplikationssatz separiert und ein separater Likelihood 


über ein geeignetes Abstandsmaß berechnet werden. 


4.2 SLAM - 2D-Rasterkarte 


In unstrukturierten Umgebungen, in denen keine charakteristischen und 
klar unterscheidbaren Objekte vorhanden sind, ist die Kartierung mit dich- 
ten Karten u.U. besser geeignet. Hierfür wird im Folgenden ein eigenes 
Verfahren vorgestellt, welches einen Ansatz für eine flexible Integration 
von Erweiterungen verfolgt. Das Verfahren basiert auf einem RBPF und 
lehnt sich stark an [Gri05] und [Gri07] an, vgl. Abschnitt 3.2.3. Es hat 


jedoch entscheidende Unterschiede, um die modulare Erweiterbarkeit zu 
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realisieren. Diese Erweiterungen werden anschließend in den folgenden 
Abschnitten 4.3 und 5.1 vorgestellt. 

Da es sich um 2D-SLAM handelt, ist der Zustand der mobilen Platt- 
form deren 2D-Pose x = [x, y, y], d.h. x-Koordinate, y-Koordinate und 
Gierwinkel yw. Es wird zunächst, wie bei dem in Abschnitt 3.2.3 erläu- 
terten Verfahren, für jedes Partikel eine Prädiktion mit den Messungen 
aus der Rad-Odometrie mit einem entsprechenden Bewegungsmodell und 
anschließender Registrierung in der zum Partikel gehörigen Karte durch- 
geführt. Neben dem generischen Bewegungsmodell aus [Thr05] ist je nach 
Roboterplattform auch die Verwendung alternativer Bewegungsmodelle 
möglich. Die Modelle umfassen hierbei zusätzlich eines für mobile Roboter 
mit Differential-Antrieb, d.h. zwei individuell angetriebene Räder senk- 
recht zum Drehpunkt und passive Schleppräder zur Abstützung, und ein 
ebenfalls generisches Modell, bestehend aus Vorwärtsgeschwindigkeit und 
Drehrate. 

Das anschließende Scan-Matching wird mit einer Methode basierend 
auf dem Bergsteigeralgorithmus realisiert. Die dabei eingesetzte Ziel- 
funktion besteht aus einem modifizierten probabilistischen Modell für 
die kleinsten Entfernungen der Strahlenendpunkte des 2D-LiDAR-Scans 
nach [Thr05]. Das modifizierte probabilistische Modell deckt zusätzlich zur 
Varianz des Messfehlers of die Tatsache ab, dass die Laserstrahlen mit der 
zurückgelegten Distanz zum LiDAR-Sensor dr divergieren und damit eine 
zusätzliche Messunsicherheit vorhanden ist, welche proportional zur Mess- 
distanz steigt. Der Messfehler of bezieht sich auf die Abstandsrichtung, 
wohingegen die Strahlaufweitung senkrecht dazu ist. Dennoch hat die 
Strahlaufweitung auch einen Einfluss auf die Entfernungsmessung, wenn 
der Einfallswinkel zur Oberfläche flach ist. Zusätzlich ergibt sich durch die 
Rasterung der Belegtheitskarte eine diskrete Abtastung mit einer entspre- 
chenden Fehlerverteilung, welche approximativ als gleichverteilt über die 


Zellengröße Z angenommen werden kann. Damit wird die Gesamt-Varianz 
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co des n-ten Laserstrahls wie folgt berechnet: 


On = OL + (lcd) + (Z)")/12, (4.29) 


mit ¢ als Faktor fiir die Strahlaufweitung, welche neben der Varianz des 
Messfehlers of aus dem Datenblatt des LiDARs entnommen werden kann. 

Es werden zunächst die euklidischen Distanzen d, von den N Strah- 
lenendpunkten zur jeweils nächsten belegten Zelle ermittelt. Der Like- 


lihood des gesamten Scans bestehend aus N Strahlen ergibt sich zu 


- BE 
PN du E l (4.30) 


Nach dem Scan-Matching wird wieder ähnlich zu dem Ansatz in 
Abschnitt 3.2.3 eine Normalverteilung der Lokalisierung geschätzt. Wäh- 
rend dort in Gleichungen (3.50) und (3.51) die Bewegungsschätzung durch 
Gewichtung bei der Mittelwertbestimmung bzw. der Konstruktion der 
Kovarianzmatrix mit eingeht, wird hier nur eine Schätzung der Lokalisie- 
rung in der Karte für jedes Partikel durchgeführt: 


J 
` plan vx sl, (4.31) 


"eh 


JA 
J 

S m 2. risch?) rX x)(x; - V(x, SR (4.32) 
ja 


mit dem modifizierten Normalisierungsfaktor 


J 
x = Yrlzlal,. si , (4.33) 


wobei DO jal. x) gerade dem Likelihood des gesamten Scans py mit 
der jeweiligen Pose x; entspricht. 
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Die Fusion der Prädiktion aus der Bewegungsschätzung mit der Loka- 
lisierung erfolgt anschließend über ein erweitertes Kalman-Filter für jedes 
Partikel, wobei hier die Messmatrix entfällt, da die Schätzung von xl! und 


RU im Zustandsraum erfolgt. 


ar at (4.34) 
l] _plil (plil oli 

Ke = Pir- dë, i +X, ) (4.35) 

a = + KEIU (4.36) 
il (1) pli] [i] [ily (él peli. 

Pil = (r-Klplil_(I-Kl 1 + Kllsilei (4.37) 


Somit geht auch die Lokalisierung in die vorgeschlagene Verteilung mit 
ein. 
Die Gewichte der Partikel lassen sich anschließend wie folgt berechnen: 


ll Li ` 
E ka li]|k-1,[i] zk yk nk SE l 
p(x x IT. z“, u“, n 1) 
wobeiessich en p(x (e per- H ux) wieder um das Bewegungsmodell handelt 


i Lab uč, ail i1) aus der verbesserten vorgeschlage- 


und sich DEM 
nen a mit den Gleichungen (4.36) und (4.37) berechnen lässt. 


4.2.1 Zusammenfassung 


Das vorgestellte SLAM-Verfahren dient in dieser Form zur Erstellung 
von dichten 2D-Rasterkarten auf Basis von Rad-Odometrie- und 2D- 
LiDAR-Daten. Der zugrundeliegende SLAM-Algorithmus ist an [Gri05] 
und [Gri07] angelehnt und es handelt sich um ein RBPF mit einer Ver- 
besserung der vorgeschlagenen Verteilung. Es unterscheidet sich jedoch 
von diesen in der Fusion mit einem EKF je Partikel für die Integration der 
Lokalisierung, wodurch eine Modularität hinsichtlich der Kombination 
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mehrerer Kartierungs- und damit zusammenhängender Lokalisierungs- 
methoden erreicht wird. Somit bietet das modulare SLAM-Verfahren die 
Möglichkeit, über das EKF pro Partikel probabilistisch konsistent wei- 
tere Sensoren oder Lokalisierungsmethoden zu integrieren, was in den 
folgenden Abschnitten vorgestellt wird. 


4.3 Kombination verschiedener Kartenarten 


Werden als Landmarken bestimme Objektarten verwendet, kann es in 
unstrukturierten Bereichen, in denen dieser Objekttyp nicht vorhanden 
ist, zu einer Verschlechterung der Lokalisierungsgenauigkeit und damit 
auch der gesamten Kartierung kommen. In gemischten Umgebungen, d.h. 
Umgebungen, die sowohl vereinzelte Landmarkenobjekte beinhalten als 
auch unstrukturierte Bereiche, kann es sinnvoll sein, verschiedene Karten- 
arten zu kombinieren. Diese Kombination verschiedener Kartenarten kann 


auch als eine Fusion auf Kartenebene interpretiert werden. 


4.3.1 Landmarkenbasierte und dichte Karte 


In unstrukturierten Bereichen sind Landmarken meist schwierig zu extra- 
hieren, weshalb hier Rasterkarten besser geeignet sein können. Im Gegen- 
satz dazu kann es in dünn besetzten Gebieten bei Rasterkarten eher dazu 
kommen, dass das Scan-Matching ungenau ist oder fehlschlägt, wohinge- 
gen einzelne Landmarken hier leichter zu detektieren sind und die Lokali- 
sierung in einer Landmarkenkarte erlauben. Dadurch, dass die Methoden 
in jeweils unterschiedlichen Bereichen eine genaue Lokalisierung erlauben, 
können sie sich durch ihre Kombination ergänzen. 

Für die Kombination verschiedener Kartierungsarten wurde ein modu- 
larer SLAM-Algorithmus entwickelt [Emt12b]. Dieser basiert auf dem in 
Abschnitt 4.2 eingeführten RBPF und jedes Partikel enthält einen EKF für 
die Berechnung und Verbesserung der vorgeschlagenen Verteilung. Hier- 


durch wird die Einbeziehung mehrerer Lokalisierungsschätzungen von 
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unterschiedlichen Karten ermöglicht. Mit der Fusion in die vorgeschla- 
gene Verteilung und damit deren Verbesserung basiert der Ansatz wei- 
terhin auf dem Konzept des FastSLAM-2.0-Algorithmus. Dabei handelt es 
sich um einen modularen Ansatz, d.h., jedes Kartierungsmodul kann als 
Komponente angesehen werden und es ist möglich, eine beliebige Anzahl 
von Kartierungsmodulen in das SLAM-Verfahren zu integrieren. Vorausset- 
zung hierfür ist, dass ein Modul neben einer Lokalisierungsschätzung die 
Möglichkeit bietet, einen Likelihood der aktuellen Beobachtungen bezüg- 
lich der zugehörigen Karte für die Bestimmung der Gewichte der Partikel 
zu berechnen [Emt12a]. 

Mit dem modularen SLAM-Algorithmus lassen sich die in den vorange- 
gangenen Abschnitten 4.1 und 4.2 vorgestellten Methoden für gemischte 
Umgebungen zu einem hybriden SLAM-Algorithmus kombinieren. Die 
Einbeziehung der Lokalisierungsschätzungen erfolgt dabei zum einen 
über die inkrementelle Verbesserung der vorgeschlagenen Verteilung mit 
den beobachteten und in der Karte zugeordneten Landmarken, wie in 
Abschnitt 4.1.2 mit den Gleichungen von (4.6) bis (4.20) beschrieben. Zum 
anderen erfolgt sie durch die Fusion der Lokalisierung in der Rasterkarte 
in dem EKF, wie in Abschnitt 4.2 mit den Gleichungen von (4.34) bis (4.37) 
erläutert. 

Die Gewichte der Partikel lassen sich für den hybriden SLAM-Algo- 
rithmus wie folgt berechnen: 

i 
ti] A0 (xt! 
D sl zk,uk,nk[l) 


SC ur) 


Pl, ` ba 


p(x! 


d.h. durch eine Kombination von Gleichung (4.21) von den erweiterten 


Landmarken und Gleichung (4.38) von der Rasterkarte. 
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4.3.2 Zusammenfassung 


Durch die konsequente Auslegung in probabilistisch konsistenter Weise 
der in Abschnitten 4.1 und 4.2 vorgestellten Verfahren können diese mit 
dem modularen SLAM-Algorithmus in einem integrierten hybriden Ansatz 
kombiniert werden, um deren gegenseitige Vorteile für eine robustere Kar- 
tierung und Lokalisierung in gemischten Umgebungen in ergänzender 
Weise zu nutzen. 
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Kapitel 5 


Integration von 
Multi-Sensor-Fusion und 
SLAM 


Bei den im vorherigen Kapitel vorgestellten SLAM-Verfahren kommt als 
Bewegungssensor lediglich Rad-Odometrie zum Einsatz. Bei der Odome- 
trie handelt es sich um einen relativ messenden Sensor, d.h., es wird die 
Differenz bzw. die Relation zwischen zwei Posen gemessen. Als relativ 
messender Sensor ist Odometrie zwar lokal sehr genau, wenn kein star- 
ker Schlupf auftritt, kann jedoch über die Zeit driften. Durch die Fusion 
von Odometrie mit einem absoluten Sensor wie einem GNSS kann der glo- 
bale Fehler bei der Lokalisierung begrenzt und somit die Drift korrigiert 
werden [Emt08]. 

Die Lokalisierung in einer Karte, welche simultan aufgenommen wird, 
unterliegt ähnlichen Problemen wie die Odometrie: Wird über lange Zeit 
unbekanntes Gebiet exploriert, vergrößert sich die Unsicherheit der Robo- 
terpose und das Schließen von Schleifen wird schwieriger. Deswegen sind 
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vor allem große Schleifen bei SLAM eine besondere Herausforderung. 
Durch die Integration von Multi-Sensor-Fusion und SLAM sollen nun die 
Vorteile der Multi-Sensor-Fusion zur Lokalisierung mit SLAM kombiniert 
werden, d.h., es werden zusätzliche absolute Sensoren integriert, um die 
Unsicherheit global zu begrenzen und damit das Schließen von Schleifen 


zu vereinfachen. 


5.1 RBPF-SLAM in Kombination mit 
Multi-Sensor-Fusion 


Je größer eine zu schließende Schleife ist, desto mehr Partikel werden i. A. 
bei den RBPF-SLAM-Algorithmen benötigt. Dies rührt daher, dass wäh- 
rend der ersten Befahrung der Schleife ständig neues Gebiet erkundet wird 
und damit die Lokalisierungsunsicherheit anwächst. Da die Partikelzahl 
begrenzt ist und durch das Resampling in dem wahrscheinlichsten Bereich 
des Zustandsraums gehalten wird, kann die anwachsende Unsicherheit 
nicht mehr adäquat abgedeckt werden, was in der englischen Literatur 
auch als particle depletion bekannt ist [Mon07]. 

Durch einen neuartigen Ansatz mit der Kombination von SLAM und 
Multi-Sensor-Fusion zur Integration eines absoluten Positionssensors soll 
die Lokalisierungsunsicherheit für ein robusteres Schließen von Schlei- 
fen begrenzt werden. Als Verfahren wird der modulare RBPF-SLAM- 
Algorithmus eingesetzt, welcher auf der FastSLAM-2.0-Methodik basiert 
und bereits in den vorangegangenen Abschnitten vorgestellt wurde. So 
wurde der modulare RBPF-SLAM-Algorithmus beispielsweise in Abschnitt 
4.2 für die Kartierung anhand von Rad-Odometrie- und 2D-LiDAR-Sensoren 
mit 2D-Rasterkarten vorgestellt und auf dessen Entwicklung in Hinblick 
auf eine Erweiterbarkeit mit zusätzlichen Sensoren und Lokalisierungsme- 
thoden eingegangen. 

Durch die Integration eines absoluten Positionssensors kann die Unsi- 
cherheit selbst bei großen Schleifen global begrenzt werden. Während 
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Multi-Sensor-Fusion im Allgemeinen zu genaueren Schätzungen führt, 
d. h., die Genauigkeit der Lokalisierung und der Karte erhöht wird, kann die 
Begrenzung der Unsicherheit bei SLAM darüber hinaus das Schließen von 
Schleifen vereinfachen und damit auch die Konsistenz der Kartierung ver- 
bessern. Da bei Partikelfilter-SLAM umso mehr Partikel benötigt werden, 
je größer die zu schließende Schleife ist, kann durch die Begrenzung der 
Unsicherheit eine Reduzierung der benötigten Anzahl der Partikel erzielt 
werden [Emt12a]. 

In Abschnitt 4.3 wurde darauf eingegangen, wie sich die Kombina- 
tion verschiedener Kartenarten dazu eignet, die Lokalisierung und Kar- 
tierung sowohl in dicht besetzten und unstrukturierten Umgebungen als 
auch in spärlich besetzten Umgebungen mit nur wenigen Landmarkenob- 
jekten zu erlauben. Sind weder Landmarken noch andere beobachtbare 
Objekte in der Umgebung, kann durch die Kombination mit einem abso- 
luten Positionssensor mittels Multi-Sensor-Fusion die Lokalisierung wei- 
terhin gestützt werden. In Outdoor-Umgebungen liegt der Einsatz eines 
GNSS als absolutem Positionssensor nahe, da es global verfügbar ist und 
sich sehr gut mit den Umgebungssensoren ergänzt. Seine Genauigkeit ist in 
offenen Gebieten mit freiem Himmel besonders hoch und kann die hier ein- 
geschränkte Lokalisierung in der Karte kompensieren. In modernen GPS- 
bzw. GNSS-Empfängern können zwar viele Fehlerquellen, wie z. B. Störun- 
gen der Ausbreitung in Iono- und Troposphäre oder Uhrenfehler, durch 
differentielle Verfahren oder die Verwendung von Korrektursignalen mini- 
miert werden, jedoch nicht lokale Phänomene wie die an hohen Objekten 
entstehende Mehrwegeausbreitung oder Abschattungen [Bra01; Bra17]. 
D.h., besonders in Gebieten mit hohen Bäumen oder urbanen Gegenden 
mit hohen Häusern ist bei einem GNSS mit größeren Fehlern zu rech- 
nen [Zha14a]. Gerade in diesen dicht besetzten Gebieten ist wiederum zu 
erwarten, dass die Lokalisierung in der Karte sehr genau ist. 

Es wird zunächst für jedes Partikel eine Prädiktion mit den Messungen 
der Rad-Odometrie und einem entsprechenden Bewegungsmodell durch- 


geführt. Die Fusion der Prädiktion aus der Bewegungsschätzung mit der 


83 


5 Integration von Multi-Sensor-Fusion und SLAM 


Messung des GNSS erfolgt anschließend über ein erweitertes Kalman-Filter 


pro Partikel im Korrekturschritt: 


si = ZGNSS,k — nl.) , (5.1) 


s e A =i: 
Kl = pl z HI ON > HI + Kos) ; (5.2) 
ali] _ fi] [i] 
Zu =X +K 


[i] _ [i] [i] leg AT Abt D: 
Pil = (1- KUn) (1- KU Hy) + Kl goes KIT. (54) 


~ [i] 
klk-1 + Sk Yk > (5.3) 


Die Messfehlerkovarianzmatrix R;, wird für die meisten Sensoren als kon- 
stant modelliert und der Index k könnte entfallen. Bei einem modernen 
GNSS-Empfänger ist Ronss,x jedoch von k abhängig, da der Empfänger 
neben der eigentlichen Positionsschätzung auch eine aktuelle Messunsi- 
cherheit mit ausgibt. In diese gehen neben der momentanen Satellitenkon- 
stellation u. A. auch die Signal-zu-Rauschabstände der Satellitensignale mit 
ein. 

Erst nach der Fusion mit den Messungen des GNSS wird für jedes Parti- 
kel das Scan-Matching in der jeweiligen Karte durchgeführt, vgl. Abschnitt 
4.2. Da hier durch die Fusion mit dem absoluten Positionssensor bereits 
eine Stützung der relativen Rad-Odometrie erfolgt ist, kann dies u. U. schon 
das Scan-Matching verbessern, da es sich dabei um ein lokales Verfahren 
handelt und eine falsche Startpose dazu führen kann, dass nur ein loka- 
les Optimum gefunden wird. Anschließend erfolgt für jedes Partikel die 
Fusion der Lokalisierung in der jeweiligen Rasterkarte mittels EKF, wie in 
den Gleichungen von (4.34) bis (4.37) erläutert. 

Die Gewichte der Partikel lassen sich schließlich wie folgt berechnen: 
‘ll, xl oy 
| GNSS,k» Eu «) Up (55) 
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wobei DEN bw A ur ux) aus der Fusion des Bewegungsmodells mit 
dem GNSS nach Gleichungen (5.3) und (5.4) berechnet wird. Die restlichen 
Komponenten werden analog zu Gleichung (4.38) berechnet. 

Durch die Einbeziehung eines globalen Positionssensors kann auch 
direkt eine globale Referenzierung der Karte erfolgen, d h., die entstehende 


Karte ist geo-referenziert. 


5.1.1 Zusammenfassung 


Durch die konsequente Auslegung in probabilistisch konsistenter Weise 
des in Abschnitt 4.2 vorgestellten modularen SLAM-Algorithmus kön- 
nen zusätzliche Lokalisierungssensoren integriert werden. Hierbei hat vor 
allem die Integration absoluter Sensoren den Vorteil, die Unsicherheit glo- 
bal zu begrenzen und damit das Schließen von Schleifen zu vereinfachen, 
was letztendlich das Erstellen konsistenter Karten erleichtert. 

In [Gui02a] wurde bereits ein hochgenaues RTK-GPS mit EKF-SLAM 
und Landmarkenkarte kombiniert. Der hier vorgestellte Ansatz integriert 
im Vergleich dazu ein GNSS in einen RBPF-SLAM-Algorithmus mit 2D- 
Rasterkarte und ist dabei nicht darauf ausgelegt, dass das GNSS eine hohe 
Genauigkeit aufweisen muss. Auch in [Shal8a] wurde ein hochgenaues 
RTK-GPS in einen RBPF-SLAM-Algorithmus und 2D-Rasterkarte inte- 
griert, wobei das GPS nur in die Gewichtung der Partikel eingeht, d. h., 
das GPS trägt nicht zur Verbesserung der vorgeschlagenen Verteilung bei 
und es müssen u. U. viele Partikel beim Resampling eliminiert werden, vgl. 
Abschnitt 3.2.1. Bei dem hier vorgestellten Ansatz nach der FastSLAM-2.0- 
Methodik geht das GNSS auch in die vorgeschlagene Verteilung ein und 


trägt somit zu ihrer Verbesserung bei. 


5.2 Stochastisches Klonen 


In den bisher vorgestellten Verfahren kommt genau ein relativer Sen- 


sor — Rad-Odometrie - zum Einsatz und dieser geht in den Prädiktions- 
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schritt ein. Sollen weitere relative Sensoren integriert werden, so können 
diese nur über einen Korrekturschritt einbezogen werden. Die Standard- 
Formulierung des EKF reicht dann zwar nicht mehr aus, da bei relati- 
ven Messungen der aktuelle Zustand von einem vorhergehenden Zustand 
abhängt. Mit der Methode des sogenannten stochastischen Klonens kön- 
nen jedoch weitere relative Sensoren formal korrekt in das EKF integriert 
werden, siehe Abschnitt 3.1.3. 


5.2.1 Implizites stochastisches Klonen 


Beim stochastischen Klonen wird der zu einer relativen Messung zugehö- 
rige Zustand mit dessen Klon erweitert. Für zwei relative Sensoren, einer 
mit höherer Rate (m-fach) für die Prädiktion und einer für den Korrektur- 
schritt mit stochastischem Klonen, kann gezeigt werden, dass ein explizites 
Klonen nicht notwendig ist [Rou02]. D. h., die augmentierte Systemkovari- 
anz Dit kann aus der geklonten Systemkovarianzmatrix Du, und den 
Zustandsübergangsmatrizen Fx;1...Fx+m rekonstruiert werden, vgl. Glei- 
chung (3.23). Die geklonte Zustandsschätzung Zut wird nicht durch die 
Prädiktionen beeinflusst und deshalb auch als statische Zustandsschätzung 
bezeichnet. Bei der Korrektur wird dann nur die aktuelle Zustandsschät- 
ZUNE Xx4m|k+m aktualisiert, vgl. Gleichung (3.31). Da hierbei nicht expli- 
zit geklont wird, soll diese Methode im Folgenden als implizites Klonen 
bezeichnet werden. 

Soll, wie im vorherigen Abschnitt dargestellt, ein zusätzlicher absoluter 
Sensor, wie z.B. ein GNSS, integriert werden, ist das implizite Klonen nur 
unter bestimmten Umständen möglich. Dabei muss die Rate des absoluten 
Sensors synchronisiert und kleiner oder gleich des zusätzlichen relativen 
Sensors sein. Außerdem muss die absolute Korrektur vor dem Klonen des 
aktuellen Zustands erfolgen. Abbildung 5.1 zeigt eine Sequenz mit synchro- 
nisierten relativen und absoluten Messungen als dynamisches bayessches 
Netz (DBN). Relative Messungen zwischen den Zuständen x; und Xk4m 


werden mit zt Gun bezeichnet und absolute Messungen zum Zeitpunkt k 


86 


5.2 Stochastisches Klonen 


mit zt. Die Zustände x, sind hellblau eingefärbt und die absoluten Mes- 


sungen zx in Orange bzw. die relativen Messungen Zx,x+m in Hellgrün. 


Abbildung 5.1: DBN mit relativen und synchronen absoluten Messungen. 


Es kann hier implizit geklont werden, wenn die absoluten Korrektu- 
ren nur dann integriert werden, während kein Klon existiert. Am Beispiel 
von Zustand x2 in Abbildung 5.1 bedeutet dies, dass zunächst die Korrek- 
tur der relativen Messung zx: durchgeführt wird, anschließend die abso- 
lute Messung zz integriert wird und erst dann das Klonen des Zustands 
für die relative Messung z2 3 erfolgt. Diese Bearbeitungssequenz ist mit 
den Zwischenschritten nochmals als Teilausschnitt in Abbildung 5.2 als 
DBN dargestellt. Nachdem der Zustand x2 durch die relative Messung zı,2 
aktualisiert wurde, wird er durch die absolute Messung zz zu x, aktua- 
lisiert. Anschließend erfolgt das Klonen von Zustand x}, wobei sich si 
gegenüber x; nicht verändert hat. Mit Zustand x; erfolgt dann die Aktua- 
lisierung durch die relative Messung z2,3 zu x3 mittels implizitem stochas- 
tischem Klonen. Diese Aktualisierung weist somit die gleiche Struktur auf 
wie das ursprüngliche stochastische Klonen mit nur einem zusätzlichen 


relativen Sensor, vgl. Abbildung 3.2. 
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ess 


Abbildung 5.2: DBN mit relativen und synchronen absoluten Messungen. Darstellung mit 
Zwischenschritten. 


Abbildung 5.3 zeigt ein DBN einer Sequenz, bei der nun asynchrone 
absolute Messungen zu verarbeiten sind. Sind die absoluten Messungen 
nicht mehr synchron, können sie auch zu Zeitpunkten auftreten, welche 
von relativen Messungen überspannt werden. Hier lässt sich die absolute 
Korrektur nicht mehr sequentiell in Zwischenschritten integrieren, wie 
dies am Beispiel von Abbildung 5.2 ausgeführt wurde. Es wäre natürlich 
möglich, relative Messungen, welche eine absolute Messung überspannen, 
auszulassen, was jedoch suboptimal ist, da nicht alle verfügbaren Informa- 
tionen genutzt werden. Im vorliegenden Fall, wie in Abbildung 5.3 darge- 
stellt, könnte keine der relativen Messungen integriert werden. 

Abbildung 5.4 zeigt einen Teil der Sequenz in einer alternativen Dar- 
stellung mit räumlichen Bezügen über die Zeit mit sich abwechselnden 
relativen und absoluten Messungen. Auch hier werden die absoluten Mes- 
sungen zum Zeitpunkt E mit az bezeichnet und relative Messungen mit 
Zk,k+m, Wobei m = 2 ist. Die prädizierten Zustandsschätzungen Zut 
und die aktualisierten Zustandsschätzungen zjx sind hellblau eingefärbt. 
Die absoluten Messungen zx sind in Orange bzw. die relativen Messungen 
Zk, Gun in Hellgrün eingefärbt. Die Pfeile zeigen hier die Abhängigkeiten 
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2 


Abbildung 5.3: DBN mit relativen und asynchronen absoluten Messungen. 


von den Messungen und die Zustandsübergänge. Die Prädiktionen von 
Zur ZU Zut Sind mit „präd. und die Korrekturschritte von Zut zu 
Sit mit „korr“ gekennzeichnet. 

Die absoluten Messungen treten in Abbildung 5.4 nicht zeitgleich mit 
den relativen Messungen auf. So korrigiert in einem Korrekturschritt die 
absolute Messung z eine potentielle Drift von X2)2. Der Korrekturschritt 
mit einer absoluten Messung hat keinen Einfluss auf vorherige Zustands- 
schätzungen und korrigiert damit auch nicht die Drift der zurückliegenden 
Schätzung Sun. Wird zum nächsten Zeitpunkt der Korrekturschritt mit der 
relativen Messung zı,; mittels stochastischem Klonen durchgeführt, wird 
diese Drift nach X3]3 weiterpropagiert und potentiell vergrößert. Mit fort- 
schreitender Zeit könnte sich diese Drift trotz Vorhandensein eines abso- 
luten Sensors unbegrenzt vergrößern und letztendlich zu einem instabilen 
Filter führen [Emt18b]. Um diesem entgegenzuwirken, sollten die Korrek- 
turen des absoluten Sensors auch den geklonten Zustand aktualisieren, 
und somit das Degenerieren des Filters unterbinden. 


89 


5 Integration von Multi-Sensor-Fusion und SLAM 


x 


Abbildung 5.4: Illustration von sich abwechselnden absoluten (zx) und relativen (Zk k+m) 
Messungen mit Ortsbezug. 


5.2.2 Explizites Klonen 


In [Mou07] wurde die Behandlung von zusätzlichen Messungen als mög- 
liche Erweiterung zu dem in Abschnitt 5.2.1 beschriebenen Ansatz des 
impliziten Klonens erwähnt. Wenn über die zwei relativen Sensoren hin- 
aus zusätzliche Sensoren zu integrieren sind, ist danach die Vereinfachung 
mit der Rekonstruktion der augmentierten Matrix Prem! x mit (3.24) und 
(3.23) nicht mehr gültig. Es muss dann explizit geklont werden und die 
augmentierte Systemkovarianzmatrix durch alle Filterschritte propagiert 
werden, es existiert jedoch - sofern bekannt - keine weiterführende Litera- 
tur hierzu. In Abgrenzung zu der Bezeichnung implizites Klonen soll diese 
Methode im Folgenden mit explizitem Klonen bezeichnet werden. 

Dass die reine Anwendung des impliziten Klonens bei zusätzlichen 
asynchronen Sensoren nicht mehr gültig ist, wurde auch im vorherigen 
Abschnitt dargestellt. Es wurde gezeigt, dass bei absoluten Messungen, wel- 
che in von relativen Messungen überspannten Zeiträumen integriert wer- 
den, die Anwendung des impliziten Klonens zu divergierenden Schätzun- 


gen führen kann. Dass beim expliziten Klonen die augmentierte Systemko- 
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varianzmatrix durch alle Filterschritte propagiert werden muss, damit auch 
die geklonte Zustandsschätzung Zum aktualisiert wird, soll im Folgenden 
dargestellt werden. 

Weil seit dem Klonen weitere Sensormessungen hinzugekommen 
sind, ist die augmentierte Systemkovarianzmatrix nach der letzten Prädik- 
tion vor dem Korrekturschritt mittels stochastischem Klonen nicht mehr 
Premik. sondern wird vom vorherigen Zeitschritt k +n — 1 im Prädiktions- 
schritt zu Pre wobei n in It. m[ liegt. 

Im Gegensatz zur Messmatrix beim Korrekturschritt mit einer relativen 
Messung mittels stochastischem Klonen (vgl. Gleichung (3.28)) ist die linke 
Submatrix von Da für eine absolute Korrektur 0, da die absolute Messung 
nur vom aktuellen Zustand abhängt: 


Han ES [0 Hl > (5.6) 


wobei 0 die Nullmatrix mit passender Dimension ist. 

Während die Submatrix K; der augmentierten Kalman-Gain-Matrix 
bei der relativen Korrektur 0 war (vgl. Gleichung (3.29)), gilt dies für eine 
absolute Korrektur nicht mehr: Kr kann nicht mehr zu 0 gesetzt werden, da 
die Messung, wie im vorherigen Abschnitt beschrieben, auch den geklon- 
ten Zustand beeinflusst. 

Die Berechnung der augmentierten Systemkovarianzmatrix Pranikan 
für eine absolute Messung zum Zeitpunkt k + n erfolgt wieder nach der 
Joseph-Form: 


v v v v u u T v vT 
Pkn|k+n = (1 = Kransen Prinitan-1 (1 — KÉ + Kr4nRx+nKxın 
6.7) 


mit der Identitätsmatrix I und der Messfehlerkovarianz Bi a, 
KD, wird mit der augmentierten Kalman-Gain-Matrix nach Glei- 


chung (3.29) und der augmentierten Messmatrix nach Gleichung (5.6) 
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berechnet: 
UBER Kx 0 Kr 
KxsnHkin = | SE [o Hian | EG ee e (5.8) 
woraus 
I- Kern Harn = d ER | (5.9) 
0 IR 
folgt. 


Aus Gleichung (5.9) wird ersichtlich, dass die linke obere Submatrix 
von P durch eine absolute Korrektur verändert wird, und das Einsetzen 
von Gleichung (5.9) in Gleichung (5.7) ergibt 


Peg Pga 


(5.10) 
Pag Paa 


Pxan|kin = 


mit den Teilmatrizen von D k+n, welche für eine bessere Nachvollzieh- 
barkeit durch ihre Indizes „g“ für den geklonten Zustand und „a“ für den 


aktuellen Zustand gekennzeichnet sind: 


Peg = Poo Fe Kx Hkn Pag = Wea(KiHisn)" 


+ Kc Hin Yaa(KrHx4n)" + KeReinK 5 (5.11) 
Pag = (I- Kan Hien — Waa(KxHiin)') + Kran Bt), (5-12) 
Poa = (Wea — Ke Hien Yaa) — Kein Hein) + KeReinKy,,, (5-13) 
Paa = (I- KkanHean) Paa (I — KrrnHkan)" + Kantin: (65.19) 


unter Verwendung der Submatrizen Y der prädizierten augmentierten Sys- 


temkovarianzmatrix 


Peg Pea 


: 5.15 
Pag Kéi ( ) 


Pk+n|k+n-1 = 
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Anhand der Gleichungen (5.11) bis (5.14) wird deutlich, dass die augmen- 
tierten Systemkovarianzmatrix P nicht wie beim impliziten Klonen mittels 
eines einfachen Produkts wie in Gleichung (3.24) mit Gleichung (3.23) zum 
Zeitpunkt k + m rekonstruiert werden kann und sie somit explizit durch 
alle Filterschritte zu propagieren ist. 

Dadurch, dass Kr nicht mehr 0 ist, ist der Klon der zur relativen Mes- 
sung gehörenden Zustandsschätzung Su. nicht mehr statisch, wie in Glei- 
chung (3.20) beschrieben, sondern wird mit der oberen Submatrix von Kian 


aktualisiert und damit zu 
Xklkın = Kklkını + KeYkan- (5.16) 


D. h., Suiten ist die geklonte Zustandsschätzung zum Zeitpunkt k mit allen 
Informationen bis zum Zeitpunkt k + n. 

Beim Korrekturschritt der relativen Messungen mittels stochastischem 
Klonen können weiterhin die Teilmatrizen wie in den Gleichungen (3.31) 
und (3.32) verwendet werden, da der geklonte Zustand danach keine wei- 
tere Verwendung findet und ein neuer Klon erzeugt wird. 

Abbildung 5.5 zeigt eine hybride Darstellung des expliziten stochas- 
tischen Klonens mit absoluten (zg) und relativen (Gr x+m) Messungen als 
DBN im oberen Teil und den Schätzungen im unteren Teil, getrennt durch 
eine gestrichelte horizontale Linie. Dabei sind die geklonten Zustands- 
schatzungen lila eingefärbt. Hier wird die Zustandsschätzung Zu nach 
der Integration der Messung z; geklont und zum Zeitpunkt k = 2 durch 
die absolute Messung z2 zu X], aktualisiert. Zusammen mit der relativen 
Messung zx: erfolgt dann zum Zeitpunkt 3 die Aktualisierung zu X3|3. Der 
alte Klon wird hierdurch obsolet und es wird die aktuelle Zustandsschät- 
zung Satz geklont. 

In Abbildung 5.6 ist in einer alternativen Darstellung mit räumlichen 
Bezügen der Einfluss des expliziten Klonens illustriert. Die Zustandsschät- 
zung Sun ist gestreift dargestellt, um darzustellen, dass die zu diesem Zeit- 


punkt aktuelle Zustandsschätzung und ihr Klon identisch sind. Wie in 
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Abbildung 5.5: Hybride Darstellung des expliziten stochastischen Klonens mit absoluten 
(zx) und relativen (Zk,k+m) Messungen als DBN im oberen Teil und den Schätzungen im 
unteren Teil, getrennt durch eine gestrichelte Linie. 


Gleichung (5.16) wird durch die absolute Messung zz sowohl San zu Satz 
aktualisiert als auch die geklonte Zustandsschätzung Zu zu Suz, Hier- 
durch wird bei der Aktualisierung mit der relativen Messung zı,3 von Satz 
zu Sais durch den Bezug auf Zu: eine geringere Drift erzeugt als in Abbil- 
dung 5.4. 


5.2.3 Partielles explizites Klonen 


In Abschnitt 3.1.3 wurde erwähnt, dass auch Ansätze existieren, welche 
explizites Klonen nur mit dem relevanten Teil der Zustandsschätzung 
durchführen, weshalb sie im Folgenden als partielles explizites Klonen 


bezeichnet werden sollen. Hierzu existieren Veröffentlichungen, welche 
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> 


x 


Abbildung 5.6: Illustration von sich abwechselnden absoluten (zg) und relativen (zx k+m) 
Messungen mit explizitem stochastischem Klonen. 


jedoch keine Analyse beinhalten, welche Auswirkungen die Verwendung 
des partiellen gegenüber dem kompletten Klonen hat [Chi11; Kle11; Ste12]. 

Zum besseren Verständnis, welche Implikationen das partielle Klonen 
gegenüber dem kompletten Klonen für die Zustandsschätzung hat, sol- 
len hier die einzelnen Schritte im Kalman-Filter anhand des kompletten 
expliziten Klonens dargestellt werden. Zur besseren Übersichtlichkeit wird 
auf die Notation mit der diskreten Zeit k verzichtet, stattdessen werden 
Bezeichner für die Submatrizen eingeführt, um diese eindeutig zu identifi- 
zieren. Die Bezeichner entsprechen der aktuellen Zustandsschätzung a, der 
relevanten geklonten Zustandsschätzung c und des ungenutzten geklonten 
Teils u, welcher beim partiellen expliziten Klonen nicht geklont wird. 

Die augmentierte Systemkovarianzmatrix wird dann zu 


Pau Puc Pua 
P=|Pu Pe Pal. (5.17) 
Pau Pac Paa 
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Im Prädiktionsschritt bleiben die geklonten Teile der Zustandsschät- 
zung nach Gleichung (3.20) unverändert und die augmentierte Zustands- 


übergangsmatrix F hat die Form 


Iw 0 0 
F=|0 I. ol, (5.18) 
0 0 F 


siehe Gleichung (3.22). Dabei sind Lan und I.. Einheitsmatrizen mit den 
entsprechenden Größen. 


Bei der Berechnung der prädizierten Systemkovarianzmatrix durch 


Puu Puc Paa F” 00 0 
FPF+Q=| Pa Poo PaF Im oo (5.19) 
FP, FPac FPaaF™| |0 0 Q 


zeigt sich, dass der ungenutzte Teil weder in den Teil der aktuellen 
Zustandsschatzung noch in den Teil des relevanten geklonten Teils eingeht, 
da nur der Block 

P, PFT 


5.20 
FP, FPF" er 


beim partiellen expliziten Klonen vorhanden ist. 
Bei dem Korrekturschritt des relativen Sensors mittels stochastischem 
Klonen werden die Elemente ebenfalls mit den genannten Bezeichnern 


versehen. Die augmentierte Messmatrix hat die Form 
H= Im. H. H,| = [o H, HI. (5.21) 


da der ungenutzte Teil gerade darüber definiert ist, dass er von der relati- 
ven Messung unberührt bleibt und somit H, = 0 gilt. Im Umkehrschluss 
bedeutet dies, dass die zum relevanten geklonten Teil korrespondierende 
Submatrix H. gerade dem Teil entspricht, an dem die linke Hälfte der 


Messmatrix nicht 0 ist. 
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Die Elemente der augmentierten Kalman-Gain-Matrix nach Gleichung 
(3.29) ändern sich mit den neuen Bezeichnern von Kx4, zu Ka und Kx 
wird aufgeteilt in den relevanten Teil K. und den ungenutzten Teil K,. Die 


Berechnung der augmentierten Kalman-Gain-Matrix ergibt sich dann zu 


Ky Puu Puc Pua 0 4 PıcH. + Pu.H, © 
K=|K.|=|Pu Pee BAH =|P.HT+P.H!|S (5.22) 
Ka Pau Pac Paa HI P.-H, + PH. 
mit 
S=HPH'+R 
=H.P..H! +H.P.H, + HaPacH, +H,P..H) +R. (5.23) 


In Gleichung (5.23) ist keine Teilmatrix mehr von dem ungenutzten Teil 
vorhanden und in Gleichung (5.22) lediglich in Ku. Nach Gleichungen (3.31) 
und (3.32) geht jedoch nur K, in den Korrekturschritt mit ein, womit der 
ungenutzte Teil keinerlei Einfluss auf den Korrekturschritt mittels stochas- 
tischem Klonen hat. 

Der Korrekturschritt mit der absoluten Messung ist ausführlich in 
Abschnitt 5.2.2 erläutert. Dort wird anhand der Gleichungen (5.11) bis (5.13) 
deutlich, dass hier durch vorhandene Kreuzkovarianzen in der Systemko- 
varianzmatrix auch der ungenutzte Teil einen Einfluss auf den geklonten 
Teil haben kann. D.h., auch wenn in dem Prädiktionsschritt und in der 
Korrektur mit dem relativen Sensor der ungenutzte Teil entfallen könnte, 
ist bei der Korrektur mit dem absoluten Sensor mit einem Unterschied 


zwischen dem partiellen und dem kompletten Klonen zu rechnen. 


5.2.4 Implizites stochastisches Klonen mit Smoothing 


Ein alternativer Ansatz, um die in dem von einer relativen Messung 
überspannten Zeitraum integrierten Messungen zum geklonten Zustand 


zurückzupropagieren, ist die Kombination mit Smoothing. Damit lassen 
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sich die Informationen rückwärts in der Zeit propagieren, womit der Vor- 
teil, die augmentierte Systemkovarianzmatrix Dt k wie beim impliziten 
Klonen bei Bedarf zu rekonstruieren erhalten bleibt [Emt18b]. 

Das Smoothing kann mit einem Rauch-Tung-Striebel-smoother (RTS- 
smoother) durchgeführt werden [Sär13]. Bei einem RTS-smoother werden 
die prädizierte Zustandsschätzung und die zugehörige Systemkovarianz- 


matrix des Standard-Kalman-Filters 


Sit = FrriRkik , (5.24) 
Prum = Eraus, + Qe (5.25) 


rückwärts in der Zeit propagiert. Die durch Smoothing geglättete Zustands- 


S 


schätzung x und die zugehörige geglättete Systemkovarianzmatrix 


k|k+1 

P% vu Können rekursiv wie folgt berechnet werden: 
Bun = Kk + Ak (Reise T kilk) (5.26) 

= T 
Priest = Prik + Ak Phris 7 Perik) Ak (5.27) 
mit 
= T p- 

Ak = Pub, Bau (5.28) 


Durch die Gleichungen (5.26) und (5.27) wird die Information der Zustands- 
schätzung zum Zeitpunkt k + 1 zur Zustandsschätzung zum Zeitpunkt k 


zurückpropagiert. Beim rekursiven Ausführen der Gleichungen vom Zeit- 


S 
k+n|k+n+1 


S 


punkt k + n aus wird zu Beginn x Kalk P 


= Xkin|ken und P 
Dit gesetzt. 

Für eine relative Messung zwischen dem Zustand zum Zeitpunkt k und 
dem Zustand zum Zeitpunkt k +m werden somit die Zustandsschatzungen 
in umgekehrter Reihenfolge rekursiv bis zum Zeitpunkt k durch Smoo- 
thing geglättet. Dadurch werden alle Informationen, welche in der Zwi- 


schenzeit durch andere Messungen integriert wurden, auf die Schätzung 
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gs S 
ZO k|k+1 
pagiert. Hierbei ist k +n = k +m — 1, da zum Zeitpunkt k + m, an dem das 


und auch die zugehörige Systemkovarianzmatrix P zurückpro- 
Smoothing durchgeführt wird, die aktuelle Zustandsschätzung die Prädik- 
tion Xk+m|k+m-ı ist und noch keine Korrektur durchgeführt wurde. 

Die augmentierte Zustandsschätzung wird dann zu 


as 

x 

e & Kita 

E le . (5.29) 
Xk+m|k+m-1 


Durch das Smoothing kann zwar die Information der absoluten Mes- 
sung zurtickpropagiert werden, der Einfluss des Korrekturschritts auf den 
erweiterten Teil der augmentierten Systemkovarianzmatrix muss jedoch 
auch bei der Rekonstruktion Berücksichtigung finden. Unter der Annahme, 
dass die Information der absoluten Messungen durch das Smoothing voll- 
ständig in die geklonte Zustandsschätzung und dessen Submatrix der aug- 
mentierten Systemkovarianzmatrix eingebracht ist, wird wie beim relati- 
ven Korrekturschritt K; gleich 0 gesetzt, vgl. Abschnitt 3.1.3. Die Berech- 
nung von Kli, bei einer absoluten Messung zum Zeitpunkt k + m 


wird dann zu 


% 2 0 0 
Kat = [o Hk | = ; (5.30) 
u ES | k+m ve 0 Klo 
woraus 
es]. 0 (5.31) 
k+m*2£k+m = 0 TaKe Hes, H 
folgt. 
Außerdem folgt aus K; = 0 
y cT 0 0 
Kx+ımRxK = 5.32 
kam RK kam = | 4 KesmRiKT,, (5.32) 
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Das Einsetzen von Gleichungen (5.31) und (5.32) in Gleichung (5.7) 
ergibt 


e P P 
P U ER > 5.33 
k+m|k+m Pae Pas (5.33) 

mit 

Peg = Yog, (5.34) 
Pag = a- Kl tom Yag > (5.35) 
Poa CS KO = KirmHxım) > (5.36) 
Paa = (I = Kita Yaa (I = KkimH isn)" + Kb, ; (5.37) 


unter Verwendung der Submatrizen Y der prädizierten augmentierten Sys- 


temkovarianzmatrix 


Peg Yea 


: 5.38 
Pag Faa ( ) 


Pk+m|k+m-1 = 


Die augmentierte Systemkovarianzmatrix Pine nach Gleichung (5.33) 
setzt sich dann aus den folgenden Teilmatrizen zusammen. Die linke obere 
Submatrix Dee = Wee bleibt wie beim ursprünglichen impliziten Klo- 
nen nach Abschnitt 5.2.1 von dem Korrekturschritt unberührt. Die rechte 
untere Submatrix Paa entspricht wieder gerade dem Korrekturschritt der 
normalen Systemkovarianzmatrix in der Joseph-Form. Die linke untere 
Submatrix Pag nach Gleichung (5.35) erhält man durch Multiplikation von 
Wag mit (I — Kk+mHk+m) von links und die rechte obere Submatrix Pga 
nach Gleichung (5.36) ist die zu ihr transponierte Matrix. Da hier nur eine 
Multiplikation stattfindet, kann analog zu Gleichung (3.24) ein modifizier- 
tes kumulatives Produkt über mehrere aufeinanderfolgende Prädiktions- 
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und Korrekturschritte gebildet werden: 


Dk = Frrm(1- Kkim-1Hk+m-1)Fk+m-1--- 
(I = Ki42Hx4+2)Frr2 (I 2 Kz+1Hg+1)Fk+1 


m-1 


= From | IO - Kuelen Er. (5.39) 


i=1 


Hierbei handelt es sich um eine vereinfachte Darstellung, bei der nur ein 
Korrekturschritt pro Prädiktion stattfindet. Bei mehreren Korrekturschrit- 
ten können diese natürlich entsprechend mit eingefügt werden. 

Die augmentierte Systemkovarianzmatrix für den Korrekturschritt der 
relativen Messung Prairies kann folglich rekonstruiert werden. Die 
linke obere Submatrix Wi vu ist die durch Smoothing geglättete System- 
kovarianzmatrix zum Zeitpunkt k, die rechte untere Submatrix entspricht 
der aktuellen Systemkovarianzmatrix und die beiden anderen Teilmatrizen 
ergeben sich durch Multiplikation der durch Smoothing geglatteten Sys- 
temkovarianzmatrix P$ mit dem modifizierten kumulativen Produkt 


k|k+1 
nach Gleichung (5.39): 


s S a 
Priks Bian tum b d (5.40) 


Penn = 
Dan. KA  Prrmiktm-ı 


Anschließend kann der Korrekturschritt mit der relativen Messung analog 
zu dem impliziten Klonen nach Gleichung (3.25) bis Gleichung (3.32) durch- 
geführt werden. D. h., die augmentierte Systemkovarianzmatrix muss auch 
hier nur temporär für diesen Korrekturschritt rekonstruiert werden. Die 
Korrekturschritte mit den absoluten Sensoren können mit der einfachen 
Zustandsschätzung und der normalen Systemkovarianzmatrix durchge- 
führt werden. 

Abbildung 5.7 zeigt eine hybride Darstellung des impliziten stochas- 
tischen Klonens mit Smoothing mit absoluten (zg) und relativen (Gr k+m) 


Messungen als DBN im oberen Teil und den Schätzungen im unteren Teil, 
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Abbildung 5.7: Hybride Darstellung des impliziten stochastischen Klonens mit Smoothing 
mit absoluten (zg) und relativen (Gi Gas) Messungen als DBN im oberen Teil und den Schät- 
zungen im unteren Teil, getrennt durch eine gestrichelte Linie. 


getrennt durch eine gestrichelte Linie. Hier wird die Zustandsschätzung 
Zum nicht explizit geklont, sondern zum Zeitpunkt 3 der benötigte Klon 


S 
112 
zı,3 erfolgt dann die Aktualisierung zu X3]3. 


x° „ durch Smoothing rekonstruiert. Zusammen mit der relativen Messung 
In Abbildung 5.8 ist in einer alternativen Darstellung mit räumlichen 
Bezügen der Einfluss des impliziten Klonens mit zusätzlichem Smoothing 
illustriert. Das Smoothing von Zu, ZU Zut) werden mit „sm“ bezeichnet. 


Bei der Integration der relativen Messung zı,; wird zunächst das Smoo- 
S 

1l2 
die Information der absoluten Messung zz zu X 


thing durchgeführt, wodurch Zu: zu x? , aktualisiert wird. Hierdurch wird 


S 
112 
Aktualisierung mit der relativen Messung Zı,3 von X3)2 zu X3j3 wird durch 


zurückgeführt. Bei der 


den Bezug auf att auch hier eine geringere Drift erzeugt als in Abbil- 
dung 5.4. 
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> 


> X 


Abbildung 5.8: Illustration von sich abwechselnden absoluten (z;) und relativen (Zk k+m) 
Messungen mit implizitem stochastischem Klonen mit Smoothing. 


Smoothing wird oft eingesetzt, um die aktuelle Schätzung mit einer 
konstanten Anzahl von zukünftigen Messungen zu verbessern, wobei die 
Ausgabe der aktuellen Schätzung dann entsprechend verzögert geschieht. 
Hierbei wird dann wegen der Verzögerung in der englischen Fachliteratur 
von fixed-lag smoothing gesprochen. Bei dem hier vorgestellten Ansatz mit 
der Kombination von implizitem Klonen und dem Smoothing kann letz- 
tere durchaus mit fixed-lag smoothing verglichen werden, da hier bzgl. der 
Zustandsschätzung Zu mit der festgelegten Anzahl m — 1 an folgenden 
Messungen geglättet wird. Es soll jedoch darauf hingewiesen werden, dass 
hierdurch keine verzögerte Ausgabe der aktuellen Zustandsschätzung ent- 
steht, da das Smoothing hier zum aktuellen Zeitpunkt k + m zurück in der 
Zeit zu k erfolgt. 

Der Ansatz mit explizitem Klonen kann für den Klon als fixed-point 
smoother bzgl. Su. interpretiert werden [Sch19]. Bei dem fixed-point smoo- 
ther wird der Ansatz verfolgt, dass ein Zustand zu einem festen Zeitpunkt 


geschätzt werden soll und auch alle zukünftigen Messungen zur Verbesse- 
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rung der Schätzung zu diesem Zeitpunkt beitragen [And79; Sim06]. Beim 
expliziten Klonen wird folglich mit der aktuellen Information aus einer 
absoluten Messung die zurückliegende geklonte Zustandsschätzung ver- 
bessert und damit die in Abschnitt 5.2.1 angesprochene Drift vermindert. 
Die fortlaufende Filterung mittels explizitem Klonen und absoluten Kor- 
rekturen kann somit als eine Kombination aus dem stochastischen Klonen 
und einem sich mitbewegenden fixed-point smoother bzgl. der geklonten 
Zustandsschätzung interpretiert werden. Der fixed-point smoother wird 
hier als sich mitbewegend bezeichnet, da der Bezugszeitpunkt immer nur 
zwischen k und k + m fest ist, weil nach jedem abgeschlossenen relativen 
Korrekturschritt neu geklont wird und sich damit auch der Bezugszeit- 


punkt verschiebt. 


5.2.5 Partielles implizites Klonen mit Smoothing 


In Anlehnung an die in Abschnitt 3.1.3 erwähnte Methode des partiellen 
expliziten Klonens, bei dem nur der relevante Teil des Zustands geklont 
wird, kann auch bei dem im vorherigen Abschnitt vorgestellten impliziten 
Klonen mit Smoothing partiell geklont werden. Hierbei wird lediglich der 
von dem relativen Sensor betroffene Teil durch Smoothing geglättet und 
die rekonstruierte augmentierte Zustandsschätzung wird zu 


x 

x _ Kita 

Sue es (5.41) 
Xk+m|k+m-1 


wobei X§ den durch Smoothing geglätteten relevanten Teil der Zustands- 
schätzung x darstellt. 

Die erweitere Systemkovarianzmatrix lässt sich nur approximativ 
rekonstruieren, da nur der relevante Teil durch Smoothing geglättet wird. 
Somit können diejenigen Teilmatrizen, welche die Kreuzkovarianzen zwi- 
schen der aktuellen Zustandsschätzung und dem geklonten Teil enthalten, 
nur näherungsweise berechnet werden. Die Teilmatrix Pac,k|k wird mit 


dem Verhältnis aus dem geklonten Teil der Systemkovarianzmatrix Pec,k|k 
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S 


und dem geglätteten geklonten Teil der Systemkovarianzmatrix Po klk+1 


gewichtet: 
-1 
Pie k|k+1 = Pack Ip. ul Perser: (5.42) 


wobei es sich bei D. tt um eine dim(x) x dim(x.)-Matrix handelt und a 
wieder für die aktuelle Zustandsschätzung steht. Durch diese Gewichtung 
entsprechen die Kreuzkovarianzen zwischen dem geklonten relevanten 


Teil und dem aktualisierten relevanten Teil in E HA dem durch Smoo- 


thing geglätteten geklonten Teil der Systemkovarianzmatrix P5 


cc,k|k+1' 
Die approximativ rekonstruierte augmentierte Systemkovarianzmatrix 


ist dann 
T yT 
3 ps |p: | F 
= k k 
LN E = ae ac,k|k+1 +m,k | (5.43) 
Fram kPic kkr Pkem|k+m-1 


Während beim partiellen expliziten Klonen nur die Kreuzkovarianzen 
zwischen dem nicht geklonten Teil und der aktuellen Zustandsschätzung 
vernachlässigt werden, werden hier zusätzlich die Kreuzkovarianzen zwi- 
schen dem geklonten Teil der Zustandsschätzung und dem nicht geklonten 


Teil der aktuellen Zustandsschätzung approximiert. 


5.2.6 Zusammenfassung 


Es wurde eine ausführliche Analyse der theoretischen Zusammenhänge 
des stochastischen Klonens durchgeführt und die Voraussetzungen zur 
Anwendbarkeit des impliziten Klonens mit zusätzlichen absoluten Mes- 
sungen dargestellt. Darauf aufbauend wurden das explizite Klonen und 
das davon abgeleitete partielle explizite Klonen eingehend analysiert. 

Mit der neuartigen Methode des impliziten Klonens mit Smoothing 
für das Zurückpropagieren von Information können zusätzliche relative 
und absolute Messungen ebenfalls robust integriert werden. Sie behält 
dabei den Vorteil des ursprünglichen impliziten Klonens, dass die augmen- 


tierte Systemkovarianzmatrix bei Bedarf rekonstruiert werden kann und 
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nicht durch alle Filterschritte propagiert werden muss. Letzteres ist bei der 
in [Mou07] als mögliche Erweiterung vorgeschlagenen Methode des expli- 
ziten Klonens und den partiellen expliziten Ansätzen der Fall. Bei diesen 
Methoden muss sowohl die Zustandsschätzung als auch die Systemkovari- 
anzmatrix explizit erweitert und durch alle Berechnungsschritte des Filters 


verarbeitet werden. 


5.3 3D-Lokalisierung und Kartierung 


Für die Integration von Multi-Sensor-Fusion und SLAM wurde in Abschnitt 
5.1 ein Verfahren für die 2D-Lokalisierung und -Kartierung mittels 2D- 
LiDAR vorgestellt. Dies ist für die Lokalisierung in Indoor-Umgebungen 
und Outdoor-Umgebungen mit flachem Gelände ausreichend und kann 
auch in unwegsamem Gelände noch bis zu einem gewissen Grad funk- 
tionieren. Wird der Untergrund allerdings zu uneben oder die Höhen- 
unterschiede während der Aufnahme der Karte zu groß, ist eine 3D- 
Lokalisierung mit allen sechs Freiheitsgraden (6DoF) und die Kartierung 
mittels 3D-LiDAR notwendig. 


5.3.1 3D-Lokalisierung 


Für die 6DoF-Posenschätzung wurde ein auf dem in Abschnitt 5.2 vor- 
gestellten SC-EKF basierendes Fusionsframework entwickelt, welches die 
Integration mehrerer absoluter und relativer Sensoren erlaubt. Das Filter ist 
modular aufgebaut und verarbeitet unter anderem die asynchronen Sens- 
ordaten einer inertialen Messeinheit (englisch: inertial measurement unit 
- IMU), welche aus drei Beschleunigungssensoren und drei Drehratensen- 
soren besteht, einem GNSS, Rad-Odometrie und kann relative 3D-Posen 
mittels stochastischem Klonen integrieren. 

Um lokale Diskontinuitäten aufgrund von geodätischen Projektionen 
zu vermeiden, wird die globale Position in geographischer Breite, geogra- 
phische Länge und Höhe über dem Referenzellipsoid (WGS84) geschätzt 
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(englisch: latitude, longitude, altitude - LLA). Die Orientierung wird von 
einer Quaternion repräsentiert. Zusätzlich zu der 6DoF-Pose enthält der 
Zustand noch Meta-Variablen, wie z.B. systematische Fehler der IMU- 
Sensoren (Biase). Der Filterzustand wird im Fehlerzustandsraum formu- 
liert (englisch: error state space), auch bekannt als indirektes Kalman-Filter 
[Rou99; Wen11]: 


Ax = [As' AO" Av' Ab}, Ab} Asodol ; (5.44) 


mit dem 3D-Positionsfehler As = [AxAyAz]" in kartesischen Koordina- 
ten, dem Lagefehler AO = [A®AYA wu] ", dem 3D-Geschwindigkeitsfehler 
Av, den Bias-Fehlern der drei Drehratensensoren Ab,, und der drei 
Beschleunigungssensoren Ab, der IMU sowie dem Skalenfaktorfehler 
für die Rad-Odometrie Asogo, was einen Zustandsvektor mit 16 Variablen 
ergibt. Bei der Formulierung im Fehlerzustandsraum werden nicht die 
Zustandsvariablen direkt geschatzt, sondern deren Fehler, weshalb diese 
Form auch als indirektes Filter bezeichnet wird. Dies hat fiir die Positi- 
onsschatzung den Vorteil, dass der Zustand zwar mit der geographischen 
Breite und Lange aufgebaut wird, die Korrektur jedoch in einem lokalen 
euklidischen Raum berechnet werden kann. Zum einen vermeidet man 
hierdurch, wie eingangs schon erwähnt, lokale Diskontinuitäten aufgrund 
von geodätischen Projektionen, zum anderen sind die Größenordnungen 
der einzelnen Zustandsvariablen nicht zu weit auseinander, was numeri- 
sche Vorteile hat [Wen11]. Durch die Verwendung des Fehlerzustandsraum 
kann die Schätzung anhand von Eulerwinkelfehlern erfolgen und dennoch 
die Orientierung durch eine Quaternion repräsentiert werden. Bei einer 
Quaternion kann es nicht wie bei der Repräsentation mittels Eulerwin- 
keln zu Singularitäten durch Mehrdeutigkeiten für einen Nick-Winkel von 
+90° kommen, was auch als gimbal-lock bekannt ist [Wen11]. Gleichzeitig 
werden numerische Probleme in der Zustandsschätzung vermieden, wel- 
che bei der Verwendung der Quaternion durch deren Überbestimmtheit 
auftreten können [Lef82]. 
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Im Prädiktionsschritt des Filters wird mit einem Strapdown-Algorith- 
mus aus den Messungen der IMU eine 6DoF-Bewegung berechnet. Die 
Beschleunigungsmesser der IMU dienen noch zusätzlich durch eine Kor- 
rektur zur Stützung der horizontalen Lage, d h. des Roll- und Nick-Winkels. 
Des Weiteren wird eine nicht-holonome Beschränkung durch einen Kor- 
rekturschritt mit einer virtuellen Messung hinzugefügt. Sie beruht auf 
dem kinematischen Modell des Fahrzeugs und trägt zu einer Stabilisierung 
quer zur Fahrtrichtung und in vertikaler Richtung bei. Die Messungen 
der Rad-Odometrie gehen als Vorwärtsgeschwindigkeit in eine Korrek- 
tur ein. Da sich das Abrollverhalten der Räder über deren Geometrie mit 
der Zeit und auch dynamisch bei Fahrmanövern ändern kann, wird hier- 
für ein zusätzlicher Skalenfaktor mitgeschätzt. Die Messungen des GNSS 
gehen in einen Korrekturschritt mit der absoluten 3D-Position und der 
3D-Geschwindigkeit ein. Der Gierwinkel kann von keinem der genannten 
Sensoren direkt gemessen werden. Deshalb wird dieser in einem Vorver- 
arbeitungsschritt aus vergangenen absoluten 2D-Positionsmessungen des 
GNSS geschätzt und geht anschließend als Korrektur ein. Alternativ kann 
der Gierwinkel mit den Daten aus einem digitalen Kompass oder einem 
GNSS mit zwei Antennen korrigiert werden, sofern diese zur Verfügung 
stehen. Die Messungen eines 3D-LiDARs gehen mittels paarweisem Scan- 
Matching aufeinanderfolgender Scans als relative 6DoF-Differenz mittels 
stochastischem Klonen in das Filter ein. Diese Art der Verwendung kann 
als LiDAR-Odometrie bezeichnet werden, analog zum Begriff visuelle Odo- 
metrie, welcher bei der Verwendung von Kameras für die Ermittlung einer 
relativen Bewegung üblich ist. Als Registrierungsverfahren wird der GICP- 
Algorithmus eingesetzt, da sich dieser speziell für das Scan-Matching von 
Scans mehrlagiger 3D-LiDAR-Sensoren eignet, siehe Abschnitt 2.2.2. 

Durch die Einbeziehung der Scans des 3D-LiDARs über den GICP- 
Algorithmus als relative Bewegung in den SC-EKF wird keine explizite 
Karte aufgebaut, es handelt sich vielmehr um eine implizite Kartierung. 
Bei Graph-basierten SLAM-Methoden werden die Scans eines 3D-LiDARs 


ebenfalls paarweise zur Berechnung einer relativen Posendifferenz ver- 
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wendet. Somit gehen sie in den Graph nur als relative Bedingung zwischen 
zwei Knoten ein und können bei späteren Optimierungen korrigiert wer- 
den, was bei einer schritthaltend aufgebauten Karte nicht direkt möglich 
wäre. Bei einem Filter ist dieser Vorteil zwar nicht vorhanden, da keine 
zurückliegenden Posen korrigiert werden. Das paarweise Scan-Matching 
ist jedoch üblicherweise mit weniger Rechenaufwand verbunden als das 
Scan-Matching mit einer Gesamtkarte, welche sich stetig vergrößert. D. h., 
diese Form der impliziten Kartierung ist auch für die Kombination mit 


einem Filter durchaus sinnvoll. 


5.3.2 3D-Kartierung 


Für die 3D-Kartierung wird die Normal Distributions Transform (NDT) 
als Repräsentation verwendet [Mag09]. Bei der NDT-Karte handelt es sich 
um eine Voxelkarte, wobei für jedes belegte Voxel eine lokale Normalver- 
teilung der darin enthaltenen Punkte geschätzt wird. Eine NDT-Karte hat 
einige Vorteile gegenüber einer reinen Belegtheitskarte, u. a. kann sie die 
Umgebung mit wesentlich größeren Voxeln adäquat repräsentieren, vgl. 
Abschnitt 3.2.4. 

Für die kontinuierliche Integration neuer Sensordaten ist eine effiziente 
inkrementelle Updatemöglichkeit unerlässlich. Im Gegensatz zu [Tak06], 
wo eine Methode für inkrementelle Updates mit nicht-korrigierter Kova- 
rianzschätzung vorgestellt wurde, wird hier analog zum ursprünglichen 
Ansatz ein effizientes Update mit einer Schätzung der korrigierten Stich- 
probenkovarianz eingesetzt, vgl. Gleichungen (3.54) und (3.55). Bei der 
inkrementellen Berechnung ergeben sich der geschätzte Mittelwert 7; und 
die geschätzte Kovarianzmatrix II; des /-ten Voxels für die Integration des 
m-ten Punkts pm nach [Tak06] wie folgt: 


; p 

ms, (5.45) 
m 

i Yim = Pi mPrm/m 

Thm = a: (5.46) 


m-1 
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mit der inkrementellen Summe der Punktkoordinaten p; und der inkre- 


mentellen Summe des dyadischen Produkts der Punkte Y; m: 


Pı,m z Pl,m-1 + Pm. (5.47) 
Yım = Yım-ı + PmPm- (5.48) 


Für die inkrementelle Berechnung werden hier somit zusätzlich die Summe 
der Punktkoordinaten p; ,, und die Summe der dyadischen Produkte der 
Punktkoordinaten Y; m benötigt. Da die Summe der dyadischen Produkte 
eine symmetrische 3 x 3-Matrix mit sechs unterschiedlichen Koeffizienten 
ist, sind zusammen mit der Summe der Punktkoordinaten und der Anzahl 
der Punkte m insgesamt zehn zusätzliche Koeffizienten pro Voxel zu spei- 
chern. Eine wesentlich speichereffizientere Variante, bei der lediglich die 
Anzahl der Punkte m pro Voxel zusätzlich zu speichern ist, kann für die 
rekursive Integration einzelner Punkte pm aus [Saa13] hergeleitet werden: 
(m = II: wu + Pm 


Tim = 2 (5.49) 
m 


d m-—2 a 1. x x 
IM; m = — M m- + — (Îl, m-1 = Pm) (Ñl, m-1 = Bal. (5.50) 
m-]1 m 


Da nur zusätzliche Elemente pro Voxel zu speichern sind, hat auch die 
Karte mit inkrementellem Update einen Speicherbedarf, welcher mit der 
Kartengröße steigt und nicht mit der Anzahl der aufgenommenen Punkte 
bzw. der Kartierungsdauer. 

In der NDT-Karte können beispielsweise durch verrauschte Sensorda- 
ten, eine fehlerhafte Lokalisierung oder auch durch dynamische Objekte 
fälschlicherweise Voxel instanziiert werden. Um diese auch wieder elimi- 
nieren zu können, ist ein Verfahren zur Einbeziehung von freiem Raum 
nach [Saa13] integriert. Hierfür wird zusätzlich jedem Voxel eine Exis- 
tenzwahrscheinlichkeit äquivalent zur Belegtheitswahrscheinlichkeit bei 
den Belegtheitskarten hinzugefügt. Diese Existenzwahrscheinlichkeit wird 


erhöht, wenn ein Punkt in dieses Voxel fällt, und verringert, wenn ein Laser- 
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strahl das Voxel durchquert. Hierfür wird jeder Laserstrahl von seinem 
Ursprung mit dem Bresenham-Strahlverfolgungsalgorithmus bis zum End- 
punkt durch die Voxel verfolgt [Bre65; Sch13]. Voxel werden erst instanzi- 
iert, wenn deren Existenzwahrscheinlichkeit über einen gewissen Schwell- 
wert steigt. So kann verhindert werden, dass Voxel durch vereinzelte Beob- 
achtungen instanziiert werden. Fällt die Existenzwahrscheinlichkeit hin- 
gegen unter einen gewissen Schwellwert, so wird dieses Voxel als unbelegt 
betrachtet. Es wird jedoch weiterhin aktualisiert, um sicherzustellen, dass 
keine Information verloren geht, sollte das Voxel bei ausreichender Evidenz 
für dessen Existenz wieder als belegt betrachtet werden. 


5.3.3 Schleifenschluss 


Das Schließen von Schleifen ist eine essentielle Eigenschaft von SLAM. 
Der GICP-Algorithmus kann bei der impliziten Kartierung nicht nur zum 
Scan-Matching zweier aufeinanderfolgender Scans, sondern auch zum 
Scan-Matching mit einem älteren Scan für einen Schleifenschluss ver- 
wendet werden. Hierfür wird für den aktuellen Scan zunächst durch die 
Bestimmung der euklidischen Distanz zu den zurückliegenden geschätzten 
Posen auf einen potentiellen Schleifenschluss überprüft. Im zweiten Schritt 
wird der GICP-Algorithmus mit dem aktuellen Scan und dem Kandidaten 
für eine Schleife durchgeführt. Die vom GICP-Algorithmus ausgegebene 
Bewertung für das Scan-Matching wird zusätzlich für die Verifikation oder 
Ablehnung der Schleife herangezogen. 

Die relative Bewegung aus dem Scan-Matching zum Schließen der 
Schleife kann allerdings nicht als relatives Update mittels implizitem sto- 
chastischem Klonen integriert werden, da zwischen den beiden Zuständen 
eine zu lange Zeit bzw. ein zu langer Weg liegt und die Annahme der Linea- 
risierung von ¥ nicht mehr gelten kann. Beim expliziten Klonen hätte 
die zurückliegende Zustandsschätzung geklont und mitpropagiert wer- 
den müssen. Da aber potentiell jeder Zustand zu einem Schleifenschluss 


gehören könnte, müssten alle Zustandsschätzungen als Klone mitgeführt 
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werden, was einen immensen Rechenaufwand bedeuten würde, da sich der 
Zustandsraum stetig vergrößert. 

Da das GNSS in der in Abschnitt 5.3.1 vorgestellten Sensorkonfigura- 
tion der einzige absolute Positionssensor ist, hat es einen maßgeblichen 
Einfluss auf die geschätzte Position. In modernen GNSS-Empfängern kön- 
nen mittels zusätzlicher Korrekturservices die meisten Fehler weitestge- 
hend korrigiert werden. Von den verbleibenden Fehlern ist die Mehrwege- 
ausbreitung der dominierende und die durch sie verursachte Abweichung 
der Positionsschätzung kann immer noch mehrere Meter betragen [Bra01; 
Bra17]. Die Mehrwegeausbreitung ist stark von der Umgebung und der 
Satellitenkonstellation abhängig. Da letztere zeitvariant ist, können sich 
die Fehler an einem Ort über die Zeit ändern, d.h. auch von dem ersten 
Besuch eines Ortes bis zum Schleifenschluss. Um nun für den SC-EKF mit 
der impliziten Kartierung einen Schleifenschluss zu realisieren, wird eine 
zusätzliche Schätzung der Abweichung der GNSS-Positionsmessungen hin- 
zugefügt. Für die Orientierung werden hingegen keine Abweichungen 
geschätzt, da diese hinreichend durch die Beschleunigungsmesser und 
den aus der Vorverarbeitung berechneten Gierwinkel stabilisiert sind. Der 
Zustandsfehlerraum aus Gleichung 5.44 wird um den Fehler der 3D-GNSS- 
Abweichung Aocnss erweitert: 


AXloop = EM Aoayss) | » (5.51) 


was dann einen Zustandsraum mit 19 Variablen ergibt. Die Abweichung 
geht beim Schleifenschluss durch die relative Position aus dem Scan- 
Matching zwischen dem aktuellen Scan und dem Scan des zuriickliegen- 
den Zustands als Messung in das Filter ein. Die Abweichung wird dann im 


Korrekturschritt des GNSS entsprechend mit einberechnet. 


5.3.4 Asynchrone Multi-Sensor-Fusion 


Das in Abschnitt 5.3.1 vorgestellte Filter ist durch seine modulare Filter- 


struktur in der Lage, verschiedene Sensorkombinationen zu verarbeiten. 
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Dabei können einzelne Sensoren auch im Betrieb zeitweise oder auch ganz 
ausfallen, ohne dass die Lokalisierung abgebrochen wird. Die Lokalisierung 
wird vielmehr nahtlos mit weniger Sensoren fortgesetzt. Die inhärente 
Kompensation des Ausfalls einzelner Sensoren geht jedoch unweigerlich 
mit einer gleichzeitigen Verschlechterung der Präzision einher, da weniger 
Informationen zur Verfügung stehen. Diese Art der Ausfalltoleranz kann 
auch unter dem Begriff graceful degradation zusammengefasst werden, da 
im Gegensatz zum plötzlichen Totalausfall eine Abnahme der erzielbaren 
Genauigkeit erfolgt [Gu16]. 

Mit dem Fusionsalgorithmus können außerdem die Daten von asyn- 
chronen Sensoren verarbeitet werden. Die asynchrone Sensordatenverar- 
beitung bezieht sich zum einen darauf, dass die einzelnen Sensoren unter- 
schiedliche Datenraten aufweisen. Zum anderen kommen die Sensorda- 
ten selbst bei gleichen Datenraten nicht gleichzeitig an, da die Taktgeber 
der Sensoren separat laufen. Die Möglichkeit der Verarbeitung von asyn- 
chronen Sensordaten bietet den Vorteil, dass keine aufwändige Hardware- 
Synchronisation der einzelnen Sensoren notwendig ist. So können neue 
oder alternative Sensoren mit wenig Aufwand integriert werden, was vor 
allem bei prototypischen Forschungsplattformen ein immenser Vorteil ist 
[Emt10b]. 


Zeitstempelschätzung 


Die einfachste Methode, um mehrere asynchrone Sensoren zu integrieren, 
ist deren direkte Verarbeitung bei Empfang der Daten im Rechner. Hierzu 
wird für die Prädiktion die spezifizierte Rate der IMU angenommen, da 
diese hier mittels Strapdown-Algorithmus integriert wird. Die Daten der 
anderen Sensoren werden der zum Zeitpunkt des Empfangs aktuellen Prä- 
diktion zugeordnet, was zu einem zusätzlichen Fehler führt, welcher durch 
entsprechende Erhöhung der Messfehlerkovarianzen modelliert wird. Da 
die Rate der IMU meist mindestens eine Größenordnung über den Raten 


der anderen Sensoren liegt, sind die zusätzlichen, in der Praxis zu erwarten- 
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den Fehler zwar gering, eine Integration der Sensordaten mit dem genauen 
Zeitpunkt ist dennoch nach Möglichkeit anzustreben. 

Für die Fusion von asynchronen Sensoren zum möglichst genauen 
Zeitpunkt bietet es sich an, die Sensordaten auf dem verarbeitenden 
Rechner mit Zeitstempeln zu versehen. Da die Sensoranbindung und 
-datenverarbeitung meist mit Rechnern durchgeführt wird, deren Betriebs- 
system keinen harten Echtzeitbedingungen genügt, können jedoch die 
Latenzen der Sensordaten schwanken. Die Latenz bezeichnet dabei die 
Zeit, welche von dem Zeitpunkt vergeht, an dem die Daten vom Sensor 
verschickt werden, bis sie im Speicher des Rechners verfügbar sind und 
mit einem Zeitstempel versehen werden. 

Für die Verbesserung der Genauigkeit der Zeitstempel bietet sich eine 
Kombination des Taktgebers des Sensors mit der Uhr des verarbeiten- 
den Rechners an. Während der Taktgeber des Sensors zwar sehr geringe 
Schwankungen in der Rate aufweist, kann er aufgrund der fehlenden 
Hardware-Synchronisation gegenüber der Rechneruhr und auch den Takt- 
gebern der anderen Sensoren driften. Die Zeitstempel der Rechneruhr 
hingegen können als absolute Referenz dienen, da hier alle Sensordaten 
zusammenlaufen, sie weisen jedoch, wie angesprochen, vergleichsweise 
starke Schwankungen auf. 

Für die Fusion des Taktgebers des Sensors und der Zeitstempel der 
Rechneruhr kommt ein Kalman-Filter zum Einsatz. Da es nur eine zu schät- 
zende Größe - den Zeitstempel - gibt, hat dieses Kalman-Filter keine Matri- 
zen, sondern beinhaltet nur skalare Operationen. Die Prädiktion erfolgt 
mit der Zeitstempeldifferenz aus dem Taktgeber des Sensors und in dem 
Korrekturschritt wird der Zeitstempel der Rechneruhr integriert. 

Da die Zeitstempel somit sehr genau geschätzt sind, kann nun der 
Strapdown-Algorithmus in der Prädiktion im Lokalisierungsfilter mit dem 
aus den Zeitstempeln ermittelten Zeitintervall erfolgen. Dies erhöht vor 
allem die Genauigkeit, wenn durch Übertragungsfehler vereinzelt Daten 


der IMU verloren gehen und ohne Berücksichtigung der Zeitstempel zu 
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falschen Zeitintervallen bei der Propagation der Lage, Geschwindigkeit 
und Position führen würden. 

Auch die Korrekturen können zum genaueren Zeitpunkt integriert wer- 
den. Anstatt sie der letzten Prädiktion zuzuordnen, kann die Prädiktion 
mit den letzten Sensordaten der IMU bis zum Zeitstempel des zu fusio- 
nierenden Sensors extrapoliert werden. Bei der nächsten Prädiktion wird 
das Zeitintervall für den Strapdown-Algorithmus mit den neuen Daten der 
IMU entsprechend verkürzt [Kle20]. 

Sind die in das Filter eingespeisten Zeitstempel stabilisiert und weni- 
ger Schwankungen unterworfen, so sind auch die Zeitstempel der vom 
Filter ausgegebenen Lokalisierung genauer. Dies hat auch für andere Algo- 
rithmen Vorteile, welche auf die Lokalisierung zurückgreifen, wie z.B. die 
inertiale Korrektur von LiDAR-Daten, welche durch die Eigenbewegung 
während der Rotation des LiDARs verzerrt werden. Für die inertiale Kor- 
rektur sollte die Lokalisierung bzw. Eigenbewegung zum einen möglichst 
genau geschätzt werden, zum anderen sollte die zeitliche Zuordnung zu 


den Einzelmessungen des LiDAR-Scans auch möglichst exakt sein. 


Kombinierte Parallelverarbeitung 


Die Verarbeitung der Messungen weist je nach Sensordatentyp durch 
mehr oder wenig aufwändige Vorverarbeitungs- oder Berechnungsschritte 
unterschiedliche Verarbeitungszeiten auf. Damit die Verarbeitung der Sens- 
ordaten in Echtzeit erfolgen kann, müssen die Verarbeitungszeiten jedes 
Sensors kleiner als die jeweilige Inverse der Rate dieses Sensors sein. Benö- 
tigt die Verarbeitung eines Sensors im Vergleich zu den anderen Sensoren 
jedoch sehr lange, wird durch die sequenzielle Filterung die Verarbeitung 
der folgenden Sensoren verzögert. Im Schnitt werden zwar weiche Echt- 
zeitbedingungen eingehalten, es kommt dadurch jedoch zu periodischen 
Verzögerungen der Ausgabe der Lokalisierungsschätzung, was z.B. für 
einen nachgelagerten Pfad- oder Trajektorienregler nachteilig ist. Ideal 
wäre, dass die Verarbeitungszeit jedes Sensors kleiner ist als die Inverse der 
Rate des schnellsten Sensors. Ist dies nicht der Fall, kann durch eine Par- 
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allelverarbeitung dennoch eine hohe Ausgaberate erzielt werden [Emt14; 
Emt18b]. 

Hierzu werden zwei Filter parallel verarbeitet. In einem Filter wer- 
den alle Sensordaten verarbeitet, wodurch dessen Lokalisierungsschatzung 
periodisch verzögert zur Verfügung steht. Die Verzögerung ist in Abbil- 
dung 5.9 in der oberen Reihe durch die Verarbeitung der Daten dargestellt, 
welche mit dem breiten grünen Rechteck gekennzeichnet sind. Sensoren 
mit kurzen Verarbeitungszeiten sind als schmale orange Rechtecke darge- 
stellt. Im zweiten Filter werden nur Sensordaten verarbeitet, deren Verar- 
beitungszeit kleiner als die Inverse der Rate des schnellsten Sensors ist. Es 
kann damit als Ausgabe-Filter für die Lokalisierungsschätzung mit kon- 
stant hoher Rate dienen, was in Abbildung 5.9 in der unteren Reihe durch 
die hellblauen Pfeile visualisiert ist. Da die Verzögerung des ersten Filters 
nur temporär ist, kann es nach der Verzögerung wieder aufholen und des- 
sen Zustandsschätzung auf das zweite Filter übertragen werden, was in 
Abbildung 5.9 durch die roten Pfeile dargestellt ist. Bei der Übertragung 
wird die Zustandsschätzung des zweiten Filters ersetzt, sodass es zu kei- 
ner Mehrfachberücksichtigung der Sensordaten, welche in beiden Filtern 
verarbeitet werden, kommt. Hierdurch kann das zweite Filter die Loka- 
lisierungsschätzung mit einer möglichst konstanten Rate ausgeben und 
durch die Übertragung dennoch die Information von Sensoren integrieren, 
welche eigentlich zu einer Verzögerung führen würden. Bedingung für 
die Übertragung des Zustands ist allerdings, dass die benötigte kumulierte 
Rechenzeit für die Integration aller Sensoren im ersten Filter gering genug 


ist, um wieder auf das zweite Filter aufholen zu können. 


5.3.5 Zusammenfassung 


Zur Implementierung der Verfahren für die reale Anwendung der vorge- 
stellten Filter-Methoden für die 6DoF-Lokalisierung und Kartierung dient 
ein Filter-Framework auf Basis eines EKF mit stochastischem Klonen. 


Durch die modulare Struktur können verschiedene Sensorkonfigurationen 
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Abbildung 5.9: Schematische Darstellung der parallelen Verarbeitung. Filter mit konstanter 
Ausgaberate in der unteren Reihe (hellblaue Pfeile). Filter mit Verzögerungen durch lange 
Berechnungszeiten in der oberen Reihe. Übertragung des Filterzustands als rote Pfeile. 


und Algorithmen kombiniert und damit auch evaluiert und miteinander 
verglichen werden. 

Für die 3D-Kartierung wird eine NDT-Karte unter Berücksichtigung 
der Existenzwahrscheinlichkeit pro Voxel eingesetzt und kann bei der Eva- 
luierung für qualitative Vergleiche herangezogen werden. 

Da bei den Filter-Methoden mit stochastischem Klonen keine explizite 
Kartierung erfolgt, findet für das Schließen von Schleifen eine neuartige 
Methode mit zusätzlicher Schätzung eines GNSS-Offsets Anwendung. 

Bei dem Einsatz auf Systemen ohne Echtzeitbetriebssystem kann die 
Schwankung der Latenzen beim Einlesen der Sensorwerte durch eine Zeit- 
stempelschätzung verringert werden und eine Parallelverarbeitung erlaubt 
die Ausgabe der Lokalisierungsschätzung mit hoher Rate, auch wenn ein- 


zelne Berechnungen eine längere Verarbeitungsdauer aufweisen. 
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Ergebnisse und 
Auswertung 


Die in den vorherigen Abschnitten vorgestellten Methoden und Verfahren 
wurden alle als Software implementiert und mit realen Sensordaten evalu- 
iert. Die Umsetzung erfolgte als plattformunabhängige C++-Bibliotheken, 
um sowohl eine reproduzierbare Evaluation mit aufgenommenen Daten 
zu ermöglichen als auch den Einsatz auf prototypischen mobilen Roboter- 
systemen für die autonome Navigation zu realisieren. 

Bei der Umsetzung wurde auf einige Open-Source-Bibliotheken zurück- 
gegriffen. Die Implementierung der Partikel- und Kalman-Filter wurde 
unter Verwendung von Eigen realisiert [Eigen]. Eigen ist eine Bibliothek 
für lineare Algebra und eignet sich besonders für Rechenoperationen mit 
Matrizen und Vektoren. Zur Verarbeitung der 3D-LiDAR-Scans wurde die 
Point Cloud Library (PCL) verwendet [Rus11]. Die PCL wurde für die Ver- 
arbeitung von Punktwolken entwickelt und bietet bereits eine Vielzahl 
von Algorithmen, wie z.B. den GICP-Algorithmus für das Scan-Matching. 
Darüber hinaus lässt sie sich mit eigenen Punkttypen und Verarbeitungsal- 


gorithmen erweitern und findet daher im Forschungsumfeld regen Einsatz. 
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So wurde auch die Umsetzung der inkrementellen NDT-Karte mittels dieser 
Bibliothek entwickelt. 

Für den Einsatz auf mobilen Robotersystemen wurden die entwickelten 
Algorithmen als Module für das Robot Operating System (ROS) umgesetzt. 
Bei ROS handelt es sich um ein Meta-Betriebssystem für Roboter, welches 
sich in der Robotikforschung als Quasi-Standard etabliert hat [ROS]. Es 
bietet eine Infrastruktur für die Kommunikation zwischen verschiedenen 
Softwaremodulen und eignet sich daher sehr gut für die Integration kom- 


plexer modularer Systeme [Qui09]. 


6.1 Erweiterte Landmarken 


Die Daten für die Evaluation des in Abschnitt 4.1 vorgestellten SLAM- 
Algorithmus mit den erweiterten Landmarken stammen von einer Power- 
bot-Plattform der Firma MobileRobots. Bei dieser Plattform handelt es sich 
um einen mobilen Roboter mit differentiellem Antrieb, welcher mit Rad- 
Odometrie-Sensoren ausgestattet ist. Zusätzlich sind ein 2D-LiDAR SICK 
LMS 200 und eine Kamera PointGrey Flea mit einer Auflösung von 640 
x 480 Pixeln für die Landmarkenextraktion installiert. Die Kamera ist mit 
einem 1/3-Zoll-Sensor und einem 8-mm-Objektiv ausgestattet, was einen 
Öffnungswinkel von etwa 33,3° ergibt. Dies bedeutet, dass die Kamera eine 
horizontale Auflösung von ca. 0,05° pro Pixel hat, was in etwa einem Zwan- 
zigstel der Winkelinkremente des Laserscanners mit 1° entspricht. 

Wie in Abschnitt 4.1 beschrieben, handelt es sich bei dem SLAM- 
Algorithmus um eine Implementierung nach FastSLAM 2.0 und als Land- 
markenmodelle dienen vertikale Strukturen. In dem Testgebiet sind meh- 
rere Laternenpfähle und Bäume vorhanden, die in dieses Landmarken- 
schema passen. Da keine Referenzlokalisierung oder -karte vorhanden ist, 
ist die folgende Evaluation vergleichender Art. 
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6.1.1 Evaluation 


Der SLAM-Algorithmus ist flexibel aufgebaut, sodass sowohl das etablierte 
einfache Punktlandmarkenmodell als auch das neuartige um ortsunabhän- 
gige Merkmale erweiterte Modell mit unterschiedlichen Partikelanzahlen 
miteinander verglichen werden können. Für die Evaluation wurden diese 


Konfigurationen verwendet: 
1. 1 Partikel mit erweitertem Modell, 
2. 1 Partikel mit Punktlandmarkenmodell, 
3. 25 Partikel mit erweitertem Modell, 
4. 25 Partikel mit Punktlandmarkenmodell. 


Da es sich um eine Implementierung nach FastSLAM 2.0 handelt und damit 
die Beobachtungen auch in die vorgeschlagene Verteilung mit eingehen, 
kann das Filter auch nur mit einem Partikel konvergieren [Mon03b]. 

Der befahrene Pfad hat in etwa eine Länge von 100m und es sind 
drei Schleifen auf der Strecke zu schließen. In dem Areal wurden 12 ver- 
schiedene Objekte als Landmarken erkannt. Darunter befinden sich Baum- 
stämme, Laternen und zwei zusätzlich vertikal aufgestellte Rohre. Das kom- 
binierte Extraktionsverfahren mit LIDAR und Kamera ist darauf ausgelegt, 
möglichst wenig Fehldetektionen zu liefern, und so wurden nur in ca. 20% 
der Daten Landmarken erkannt. Hierbei spielt natürlich auch der rela- 
tiv kleine Öffnungswinkel der Kamera eine Rolle, da dadurch nur selten 
Landmarken überhaupt beobachtbar sind. Das kombinierte Extraktions- 
verfahren ist sehr robust, es gab nur eine einzige Fehldetektion, bei der 
ein Busch einmalig fälschlicherweise als Landmarke erkannt wurde. Vor 
allem die Messung des Landmarkenradius ist sehr genau mit einer durch- 
schnittlichen Abweichung des geschätzten Radius über alle Landmarken 
von 7,3 mm. Der jeweilige Referenzradius wurde über den per Messung 


mit einem Maßband bestimmten Umfang berechnet. 
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Die folgenden Abbildungen zeigen die Ergebnisse der Evaluation am 
Ende der jeweiligen Kartierung. Der mobile Roboter wurde von links nach 
rechts gefahren und in drei Schleifen wieder zurück. Sein Pfad ist in Hell- 
blau und seine Endposition mit einem Kreis dargestellt. Die Landmarken 
werden durch ihre für die Darstellung vergrößerten o-Ellipsen in Grün 
repräsentiert und sind durchnummeriert. Im Falle von mehreren Partikeln 
ist die Position und die Karte des besten Partikels dargestellt, da davon 
ausgegangen werden kann, dass das Partikel mit dem höchsten Gewicht 
auch die beste Karte besitzt. 
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Abbildung 6.1: Kartierungsergebnis der Konfiguration mit einem Partikel und dem erweiter- 
ten Landmarkenmodell. Der Pfad ist in Hellblau dargestellt und die Landmarken A in Grün. 


In Abbildung 6.1 ist das Ergebnis der Konfiguration mit einem Parti- 
kel und dem erweiterten Landmarkenmodell dargestellt. In der Karte sind 
genau 12 Landmarken kartiert worden, wobei die Landmarken mit kleinen 
Fehlerellipsen deutlich öfters beobachtet wurden als die mit den größeren 
Ellipsen. Landmarke A; ist die oben angesprochene Fehldetektion, bei der 
ein Busch beobachtet wurde. 

Abbildung 6.2 zeigt das Ergebnis mit ebenfalls nur einem Partikel, 
jedoch ohne erweitertes Landmarkenmodell, was bedeutet, dass die Land- 
marken nur über ihren Ort definiert sind. Es sind insgesamt 16 Land- 


marken kartiert worden, wobei vier Landmarken fälschlicherweise neu 
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Abbildung 6.2: Kartierungsergebnis der Konfiguration mit einem Partikel und dem einfa- 
chen Punktlandmarkenmodell. Der Pfad ist in Hellblau dargestellt und die Landmarken A in 
Grün. 


instanziiert worden sind, da sie nicht zugeordnet werden konnten. Außer- 
dem traten auch Fehlzuordnungen auf, was aus der Karte jedoch nicht 
ersichtlich ist. So wurde z.B. direkt am Anfang Landmarke A, nicht gleich 
instanziiert, sondern deren Beobachtung zunächst mehrmals der nahe bei 
ihr liegenden Landmarke A, zugeordnet. Im späteren Verlauf konnte die 
erste Schleife nicht erfolgreich geschlossen werden, was an der zusätzli- 
chen Landmarke As zu erkennen ist. Auch die zweite Schleife konnte nicht 
erfolgreich geschlossen werden und es wurden durch den großen Fehler in 
der Posenschätzung die Beobachtungen den Landmarken A, und Aı nicht 
korrekt zugeordnet, sondern als Landmarken A}, und 442 fälschlicherweise 
neu instanziiert. Die letzte Schleife wurde mit den Landmarken 411 und 412 
geschlossen anstatt Ap und Ju. Letztendlich ist die erzeugte Karte aufgrund 
der Fehlzuordnungen und zusätzlichen Landmarken inkonsistent. 

Das Ergebnis von 25 Partikeln mit erweitertem Modell wird hier nicht 
gezeigt, da in der Karte keine Unterschiede zu der Variante mit nur einem 
Partikel in Abbildung 6.1 zu erkennen sind. Abbildung 6.3 zeigt das Ergeb- 
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Abbildung 6.3: Kartierungsergebnis der Konfiguration mit 25 Partikeln und dem einfachen 
Punktlandmarkenmodell. Der Pfad ist in Hellblau dargestellt und die Landmarken A in Grün. 


nis der Konfiguration von 25 Partikeln mit dem einfachen Punktlandmar- 
kenmodell. Die hier erstellte Karte enthält korrekterweise 12 Landmarken 
inklusive der Fehldetektion und es wurde keine Landmarke fälschlicher- 
weise neu instanziiert. Es traten dennoch einige Fehlzuordnungen auf, wie 
z.B. wieder gleich zu Beginn bei Beobachtungen der Landmarken A, und 
Aı, was jedoch dank des Partikelfilters im späteren Verlauf korrigiert wer- 
den konnte. Auch die Schleifen konnten erfolgreich geschlossen werden. 
Somit war es möglich, mit mehreren Partikeln auch ohne das erweiterte 
Landmarkenmodell eine konsistente Karte zu erstellen. 

Für die Bewertung der erzielten Genauigkeiten stand keine Referenz- 
lokalisierung oder Karte zur Verfügung. Deshalb konnte nur für einen Teil 
der Karte eine quantitative Evaluation durchgeführt werden. Bei den Land- 
marken 411, Ag und A, handelt es sich um Straßenlaternen, welche in einer 
Reihe entlang einer geraden Straße stehen und es kann davon ausgegangen 
werden, dass sie entlang einer Gerade ausgerichtet sind. Für die Evalua- 
tion wurde jeweils in jeder Karte eine lineare Ausgleichsgerade nach der 
Methode der kleinsten Quadrate (englisch: least-squares) durch diese Land- 
marken gelegt und die mittlere Distanz der Landmarken von dieser Gerade 
ermittelt. Eine Überprüfung der Straßenlaternen anhand eines Luftbildes 
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nach der gleichen Methode ergab einen Mittelwert der Abstände zur Aus- 
gleichsgeraden von 2mm. Für den Vergleich wurden nur die konsistenten 
Karten herangezogen und die Ergebnisse sind in Tabelle 6.1 aufgelistet. 
Anhand von Tabelle 6.1 ist zu erkennen, dass mit dem erweiterten Modell 
eine höhere Genauigkeit gegenüber dem einfachen Punktlandmarkenmo- 


dell erzielt werden konnte. 


Tabelle 6.1: Mittelwert der Distanzen zur Geraden in m. 


1 Partikel mit erweitertem Modell 0,22 
25 Partikel mit Punktlandmarkenmodell 0,51 


25 Partikel mit erweitertem Modell 0,19 


6.1.2 Zusammenfassung 


Die vergleichende Evaluation hat gezeigt, dass ein erweitertes Landmar- 
kenmodell mit ortsunabhängigen Merkmalen zur Robustheit der Datenzu- 
ordnung beitragen kann. Durch die verbesserte Datenzuordnung wurden 
bei dem SLAM-Algorithmus weniger Partikel für das erfolgreiche Schlie- 
Ben der Schleifen benötigt und somit das Erstellen einer konsistenten Karte 
vereinfacht. Vor allem war bei der Verwendung von nur einem Partikel eine 
korrekte Kartierung nur mit den zusätzlichen ortsunabhängigen Merkma- 
len möglich. Weiterhin konnte gezeigt werden, dass bei der Verwendung 
von mehreren Partikeln die Präzision der Kartierung durch eine gegen- 
über dem einfachen Punktlandmarkenmodell verbesserte Datenzuordnung 
gesteigert werden kann. 

Ein Grund für das schlechtere Abschneiden des Algorithmus ohne 
Zusatzmerkmale ist zum Beispiel, dass die Datenzuordnung bereits am 
Anfang der Fahrt zwei dicht beieinanderliegende Landmarken zeitweise 
falsch zuordnete, was im Folgenden auch zu einer schlechteren Lokali- 
sierung des Roboters führte. Durch eine zu ungenaue Lokalisierung und 
weitere Fehlzuordnungen wurde das erfolgreiche Schließen der Schleifen 


erschwert bzw. im Falle von nur einem Partikel sogar ganz verhindert. 
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6.2 SLAM - 2D-Rasterkarte 


Um den modularen SLAM-Algorithmus mit der 2D-Belegtheitskarte nach 
Abschnitt 4.2 zu evaluieren, wird dieser zunächst mit einem etablierten 
Algorithmus anhand von öffentlich verfügbaren Datensätzen verglichen. 
Die verwendeten Datensätze sind frei erhältlich und können von der 
Internetseite http://www2.informatik.uni-freiburg.de/-stachnis/ 
datasets.html heruntergeladen werden [CS]. Sie beinhalten alle Mes- 
sungen von der Rad-Odometrie des jeweils verwendeten mobilen Roboters 
und eines 2D-LiDARs mit 180° Öffnungswinkel. Die Modularität des vorge- 
stellten SLAM-Algorithmus kommt bei diesen Datensätzen noch nicht zum 
Tragen, da in den Datensätzen keine Messungen von weiteren Sensoren 
enthalten sind. So soll hier zunächst nur die grundlegende Leistungsfähig- 
keit evaluiert werden. 

Abbildung 6.4 zeigt die 2D-Belegtheitskarte des Datensatzes, welcher 
von Dirk Hähnel im Intel Research Lab aufgenommen wurde, nur unter 
Verwendung der Rad-Odometrie. Die Belegtheitswahrscheinlichkeit geht 
von frei (0 bzw. schwarz) über unbekannt (0,5 bzw. grau) bis belegt (1 bzw. 
weiß). Zu Beginn werden alle Zellen der Karte mit einer Belegtheitswahr- 
scheinlichkeit von 0,5 initialisiert, daam Anfang noch keinerlei Informa- 
tion über die Umgebung existiert. Der gefahrene Pfad des mobilen Roboters 
ist in Hellblau dargestellt. Anhand dieser Karte wird deutlich, dass ohne 
eine simultane Kartierung keine konsistente Karte erzeugt werden kann. 
Da die Rad-Odometrie nur relative Bewegungen misst und dazu meist 
auch sehr ungenau ist, können sich Fehler über die Zeit akkumulieren. So 
entstehen schon über relativ kurze Abstände gekrümmte Gänge und im 
Gesamten eine fehlerhafte und inkonsistente Karte. 

Im Folgenden werden die Ergebnisse des in Abschnitt 4.2 beschrie- 
benen modularen SLAM-Algorithmus vorgestellt und mit Referenzkarten 
verglichen. Die hier gezeigten Referenzkarten wurden anhand der mit dem 
GMapping-Algorithmus von Giorgio Grisetti und Cyrill Stachniss erzeug- 
ten Lokalisierungsergebnisse erstellt [Gri07]. Diese können ebenfalls von 
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Abbildung 6.4: 2D-Belegtheitskarte des Intel Research Lab und Pfad des mobilen Roboters 


in Hellblau, aufgezeichnet nur unter Verwendung der Rad-Odometrie. Der Grauwert bildet 
die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


der o. g. Internetseite heruntergeladen werden. Auf einen quantitativen 
Vergleich wurde aufgrund von nicht vorhandener Grundwahrheit verzich- 
tet. 


6.2.1 Büroumgebung 


Der Datensatz vom Intel Research Lab, welcher schon für Abbildung 6.4 
herangezogen wurde, beinhaltet eine typische Büroumgebung mit Gän- 
gen und vielen kleinen Räumen, jedoch mit einer Besonderheit in Form 
einer großen Schleife, weshalb der Datensatz für die Evaluation von SLAM- 
Algorithmen besonders geeignet ist. Die Büroumgebung hat eine Grund- 
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fläche von in etwa 28 x 28 m? und die Aufnahme weist ein Pfadlänge von 
etwas über 500 m auf. 
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Abbildung 6.5: Referenzkarte des Intel Research Lab und Pfad des mobilen Roboters in Hell- 
blau, berechnet mit dem GMapping-Algorithmus mit 15 Partikeln [CS]. Der Grauwert bildet 
die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Abbildung 6.5 zeigt die Referenzkarte. In [Gri07] wurde diese Karte mit 
15 Partikeln erzeugt. 

Abbildung 6.6 zeigt die mit dem modularen SLAM-Algorithmus mit 
acht Partikeln erzeugte Karte und den geschätzten Pfad zum Vergleich. 
Es konnte zwar bereits mit einem Partikel eine konsistente Karte erzeugt 
werden, bei der jedoch noch leichte Ungenauigkeiten vorhanden sind. 
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Abbildung 6.6: 2D-Belegtheitskarte des Intel Research Lab und Pfad des mobilen Roboters 
in Hellblau, berechnet mit dem modularen SLAM-Algorithmus mit acht Partikeln. Der Grau- 
wert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


6.2.2 Lange Gänge 


Der Datensatz des MIT Killian Court wurde von Mike Bosse und John Leo- 
nard aufgenommen. Dieser Datensatz enthält eine Umgebung mit vielen 
langen geraden Gängen auf einer Grundfläche von etwa 250 x 215m? und 
die Aufnahme weist ein Pfadlänge von etwas über 1,9 km auf. Der Daten- 
satz ist besonders anspruchsvoll, weil er viele verschachtelte Schleifen 
beinhaltet und die langen geraden Gänge die longitudinale Lokalisierung 
mittels Scan-Matching erschweren. Zum einen weist die Lokalisierung 


129 


6 Ergebnisse und Auswertung 


in Längsrichtung kein klares Maximum auf und zum anderen kann es in 
einem Gang, der neu kartiert wird, dazu kommen, dass die Lokalisierung 
zurück in den bereits kartierten Teil gezogen wird [Sta08]. 
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Abbildung 6.7: Referenzkarte des MIT Killian Court und Pfad des mobilen Roboters in Hell- 
blau, berechnet mit dem GMapping-Algorithmus mit 200 Partikeln [CS]. Der Grauwert bildet 
die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Abbildung 6.7 zeigt die Referenzkarte, berechnet mit dem GMapping- 
Algorithmus mit 200 Partikeln. In [Gri07] konnten mit diesem Datensatz 
konsistente Karten mit 60 Partikeln erzeugt werden, welche aber noch 
Ungenauigkeiten wie doppelte Wände aufwiesen. Ab einer höheren Anzahl 
von 80 Partikeln traten diese Ungenauigkeiten nicht mehr auf. 

Abbildung 6.8 zeigt die mit dem modularen SLAM-Algorithmus mit 
100 Partikeln erzeugte Karte und den geschätzten Pfad. Dabei konnten alle 
Schleifen erfolgreich geschlossen und eine konsistente und präzise Karte 
erzeugt werden. 
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Abbildung 6.8: 2D-Belegtheitskarte des MIT Killian Court und Pfad des mobilen Roboters in 
Hellblau, berechnet mit dem modularen SLAM-Algorithmus mit 100 Partikeln. Der Grauwert 
bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


6.2.3 Außengelände 


Der Datensatz des Freiburg Campus wurde von Cyrill Stachniss und Gior- 
gio Grisetti aufgenommen. Dieser Datensatz enthält eine Aufnahme eines 
Außenbereichs mit unterschiedlichen Bereichen. Das Areal erstreckt sich 
über ca. 250 x 250m? und die gesamte Pfadlänge beträgt etwa 1,7 km. In 
dem Areal sind große Gebäude und andere Strukturen vorhanden, aber 
auch unstrukturierte Objekte, wie Hecken oder Bäume. Außerdem gibt 
es Bereiche, in denen kaum Objekte vorhanden sind und damit fast keine 
Beobachtungen möglich sind, da der 2D-LiDAR nur einen Sichtbereich von 
180° nach vorne hat. Wie im vorherigen Datensatz sind mehrere verschach- 
telte Schleifen vorhanden. 
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Abbildung 6.9: Referenzkarte des Freiburg Campus und Pfad des mobilen Roboters in Hell- 
blau, berechnet mit dem GMapping-Algorithmus mit 100 Partikeln [CS]. Der Grauwert bildet 
die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Abbildung 6.9 zeigt die Referenzkarte, berechnet mit dem GMapping- 
Algorithmus mit 100 Partikeln. In [Gri07] konnten mit diesem Datensatz 
Karten mit guter Qualitat bereits ab 30 Partikel erzeugt werden. 

Abbildung 6.10 zeigt die mit dem modularen SLAM-Algorithmus 
erzeugte Karte und den geschätzten Pfad. Die dargestellte Karte wurde mit 
100 Partikeln erzeugt. Es konnten alle Schleifen erfolgreich geschlossen 
werden und eine konsistente und präzise Karte erzeugt werden. 
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Abbildung 6.10: 2D-Belegtheitskarte des Freiburg Campus und Pfad des mobilen Roboters in 
Hellblau, berechnet mit dem modularen SLAM-Algorithmus mit 100 Partikeln. Der Grauwert 
bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


6.2.4 Zusammenfassung 


Mit dem modularen SLAM-Algorithmus konnte die erfolgreiche Kartie- 
rung mittels Rad-Odometrie und 2D-LiDAR anhand von öffentlich verfüg- 
baren Datensätzen gezeigt werden. Dabei konnten auch sehr anspruchs- 
volle Datensätze mit großen und mehreren verschachtelten Schleifen in 
Umgebungen mit langen gerade Gängen oder auch im Außengelände mit 
unstrukturierten Bereichen kartiert werden. 

Die zum Vergleich herangezogenen Referenzkarten stammen von 
einem etablierten SLAM-Verfahren und in der qualitativen Gegenüber- 
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Abbildung 6.11: Kartierungsergebnis, berechnet mit dem modularen SLAM-Algorithmus nur 
mit erweitertem Landmarkenmodell. Der Pfad ist in Hellblau dargestellt und die Landmarken 
in Grün. Der Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


stellung konnten mit dem modularen SLAM-Algorithmus mit ähnlichen 
Anzahlen von Partikeln gleichwertige Karten erstellt werden. 

Die Modularität des vorgestellten SLAM-Algorithmus kam, wie bereits 
angemerkt, bei diesen Datensätzen noch nicht zum Tragen, sondern wird 
in den folgenden Evaluationen mit den erweiterten Methoden relevant. 


6.3 Kombination von Landmarken und 
Rasterkarte 


Wie in Abschnitt 4.3 dargestellt, werden hier die beiden Ansätze für 
die Kartierung mit erweiterten Landmarkenmodellen und für die 2D- 
Rasterkartierung in einem hybriden SLAM-Algorithmus kombiniert. 


134 


6.3 Kombination von Landmarken und Rasterkarte 


Bei dem zur Evaluation herangezogenen Datensatz handelt es sich um 
den gleichen wie in Abschnitt 6.1. Das Areal, in dem die Daten aufgenom- 
men wurden, beinhaltet neben Laternenmasten und Baumstämmen als 
Landmarkenobjekte auch geradlinige Strukturen wie Gebäudeteile sowie 
unstrukturierte Bereiche mit Bäumen und Büschen. 


6.3.1 Evaluation 


Wie in Abschnitt 4.3 dargestellt, kann der modulare SLAM-Algorithmus 
flexibel mit unterschiedlichen Kartenarten und auch deren Kombination 
genutzt werden. Für die Evaluation wurden diese Konfigurationen mitein- 


ander verglichen: 
1. nur mit Landmarken, 
2. nur mit Rasterkarte, 
3. Kombination von Landmarken und 2D-Rasterkarte. 


Abbildung 6.11 zeigt das Kartierungsergebnis, welches mit dem modu- 
laren SLAM-Algorithmus nur mit dem erweiterten Landmarkenmodell für 
die Kartierung und Lokalisierung berechnet wurde. Der geschätzte Pfad ist 
in Hellblau dargestellt und die Landmarken als grüne Kreuze. Zusätzlich 
ist die 2D-Belegtheitskarte angezeigt, wobei diese hier nur dem visuellen 
Vergleich mit den anderen Konfigurationen dient und in dieser Konfigura- 
tion nicht zur Lokalisierung verwendet wurde. Dabei ist die Belegtheits- 
wahrscheinlichkeit wie in Abschnitt 6.2 von frei (0 bzw. schwarz) über 
unbekannt (0,5 bzw. grau) bis belegt (1 bzw. weiß) dargestellt. 

Abbildung 6.12 zeigt das Kartierungsergebnis, berechnet mit dem 
modularen SLAM-Algorithmus nur mit der 2D-Rasterkarte für die Kartie- 
rung und Lokalisierung. Diese Konfiguration entspricht der Evaluation 
im vorherigen Abschnitt. Hier sind nur die drei Landmarken eingezeich- 
net, welche schon in Abschnitt 6.1 zur Evaluation verwendet wurden. 
Die Landmarken fanden in dieser Konfiguration zwar keine Verwendung 
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Abbildung 6.12: Kartierungsergebnis, berechnet mit dem modularen SLAM-Algorithmus nur 
mit 2D-Rasterkarte. Der Pfad ist in Hellblau dargestellt und die Landmarken in Grün. Der 
Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


für die Lokalisierung oder Kartierung, sollen jedoch später ebenfalls zur 
Evaluation herangezogen werden. Deshalb wurden sie hier anhand der 
Lokalisierung rekonstruiert. 

Abbildung 6.13 zeigt das Kartierungsergebnis, berechnet mit dem 
modularen SLAM-Algorithmus und der hybriden Konfiguration, d.h. mit 
der Kombination von erweitertem Landmarkenmodell und 2D-Rasterkarte, 
wobei in dieser Konfiguration auch beides für die Kartierung und Lokali- 
sierung genutzt wird. 

Alle drei Karten sehen sich sehr ähnlich und mit allen Konfigurationen 
konnten konsistente Karten erstellt werden. In Ermangelung einer Refe- 
renzkarte oder -lokalisierung werden im Folgenden qualitative Vergleiche 
der erzielten Genauigkeiten anhand von Detailbetrachtungen herangezo- 
gen. 
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Abbildung 6.13: Kartierungsergebnis, berechnet mit dem modularen SLAM-Algorithmus mit 
hybrider Konfiguration. Der Pfad ist in Hellblau dargestellt und die Landmarken in Grün. Der 
Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


In Abbildung 6.14 sind drei Detailansichten eines Eingangs zu einem 
Fahrradunterstand dargestellt, wobei Abbildung 6.14a das Ergebnis der 
Kartierung nur anhand der erweiterten Landmarken zeigt, Abbildung 6.14b 
das Ergebnis der Kartierung nur anhand der 2D-Rasterkarte und Abbil- 
dung 6.14c das Ergebnis der hybriden Konfiguration. Während bei dem 
modularen SLAM-Algorithmus nur mit dem erweitertem Landmarken- 
modell aufgrund von Ungenauigkeiten die Wände mehrfach als Doppel- 
strukturen vorhanden sind bzw. nur mit der 2D-Rasterkarte noch leichte 
Ungenauigkeiten existieren, konnte mit dem SLAM-Algorithmus mit der 
hybriden Konfiguration eine sehr viel genauere Kartierung erzielt werden 
und die Wände des Fahrradunterstands sind klarer zu erkennen. 

In Abbildung 6.15 sind drei Detailansichten eines Busches dargestellt, 
wobei Abbildung 6.15a das Ergebnis der Kartierung nur anhand der erwei- 
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Abbildung 6.14: Detailansicht eines Eingangs: Modularer SLAM-Algorithmus mit erweiter- 
tem Landmarkenmodell (a), modularer SLAM-Algorithmus mit 2D-Rasterkarte (b) und modu- 
larer SLAM-Algorithmus mit hybrider Konfiguration (c). Der Grauwert bildet die Belegtheits- 
wahrscheinlichkeit von 0 bis 1 ab. 
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terten Landmarken zeigt, 6.15b das Ergebnis der Kartierung nur anhand 
der 2D-Rasterkarte und 6.15c das Ergebnis der hybriden Konfiguration. 
Hier ist kein so deutlicher Unterschied zu erkennen wie in Abbildung 
6.14. Dennoch sind bei dem modularen SLAM-Algorithmus nur mit dem 
erweitertem Landmarkenmodell bzw. nur mit der 2D-Rasterkarte leichte 
Ungenauigkeiten zu erkennen, während der Busch mit dem modularen 
SLAM-Algorithmus mit der hybriden Konfiguration eine klarere Kontur 
aufweist. 

Für eine quantitative Evaluation wurden auch hier wieder die Land- 
marken der Straßenlaternen 411, As und Az herangezogen (siehe Abschnitt 
6.1). Es wurde wieder jeweils in jeder Karte eine lineare Ausgleichsgerade 
durch diese Landmarken gelegt und die mittlere Distanz der Landmarken 
von dieser Gerade ermittelt. Die Ergebnisse sind in Tabelle 6.2 aufgelistet. 
Anhand der Tabelle ist zu erkennen, dass die Genauigkeit mit der hybri- 
den Kartierung gegenüber den beiden Einzelansätzen gesteigert werden 
konnte, wobei sich gegenüber der Konfiguration nur mit der Rasterkarte 


lediglich ein leichter Unterschied zeigt. 


Tabelle 6.2: Mittelwert der Distanzen zur Geraden in m. 


Nur mit Landmarken 0,22 
Nur mit Rasterkarte 0,06 
Kombination von Landmarken und Rasterkarte 0,04 


6.3.2 Zusammenfassung 


Mit der Evaluation konnte gezeigt werden, dass mit dem modularen SLAM- 
Algorithmus mit der hybriden Konfiguration gegenüber den Einzelver- 
fahren eine verbesserte Kartierung und damit auch Lokalisierung erzielt 
werden konnte. Dies ist besonders deutlich im Vergleich zu dem SLAM- 
Algorithmus mit den Landmarken. Wie schon in Abschnitt 6.1 erwähnt, 


wurden nur in ca. 20% der Daten Landmarken extrahiert, was zum Teil in 
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yinm 


Abbildung 6.15: Detailansicht eines Busches: Modularer SLAM-Algorithmus mit erweiter- 
tem Landmarkenmodell (a), modularer SLAM-Algorithmus mit 2D-Rasterkarte (b), modula- 
rer SLAM-Algorithmus mit hybrider Konfiguration (c). Der Grauwert bildet die Belegtheits- 
wahrscheinlichkeit von 0 bis 1 ab. 
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dem spärlichen Vorhandensein der Landmarkenobjekte und zum Teil in 
dem relativ kleinen Öffnungswinkel der verwendeten Kamera-Objektiv- 
Kombination begründet liegt. Demgegenüber hat der 2D-LiDAR mit 180° 
einen wesentlich größeren Öffnungswinkel und es stehen in allen Berei- 
chen des Areals valide Messungen zur Kartierung und Lokalisierung zur 
Verfügung. 

Umgekehrt kann jedoch auch der SLAM-Algorithmus mit der hybri- 
den Konfiguration gegenüber dem SLAM-Algorithmus nur mit der 2D- 
Rasterkarte ebenfalls eine leichte Verbesserung erzielen. Dies liegt darin 
begründet, dass die Lokalisierung und Kartierung in einer unstrukturier- 
ten Umgebung mit einer Rasterkarte höhere Ungenauigkeiten aufweisen, 
welche durch die Hinzunahme von Landmarkenobjekten leicht verringert 


werden können. 


6.4 Kombination von Multi-Sensor-Fusion 
und SLAM mit 2D-Rasterkarte 


Die in Abschnitt 5.1 vorgestellte Kombination von Multi-Sensor-Fusion 
und SLAM mit 2D-Rasterkarte soll anhand der Integration eines zusätzli- 
chen absoluten Sensors in Form eines GNSS evaluiert werden. Mit dem 
schon in den vorherigen Ergebnisabschnitten verwendeten modularen 
SLAM-Algorithmus können flexibel weitere Sensoren, wie z.B. ein GNSS 
oder ein Kompass, in die Schätzung integriert werden. 

Der für die Evaluierung herangezogene Datensatz wurde mit einer 
geländegängigen Plattform mit vier angetriebenen Rädern und einer 
Allradlenkung aufgenommen. Diese Plattform ist mit einem MTi-G-700 
von Xsens ausgestattet, welches sowohl eine IMU als auch einen GNSS- 
Empfänger beinhaltet, wobei für diese Evaluation nur die GNSS-Daten 
verwendet wurden. Des Weiteren verfügt die Plattform über einen Rad- 
Odometriesensor und einen 3D-LiDAR Velodyne HDL-64E, wobei von 
letzterem nur ein horizontaler 2D-Ausschnitt für die Evaluation genutzt 
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wurde. Da kein absoluter Sensor für die direkte Messung des Gierwinkels 
vorhanden ist, wird diese in einem Vorverarbeitungsschritt aus vergange- 
nen 2D-Positionsmessungen des GNSS geschätzt und geht als virtueller 
Kompass-Sensor mit ein. Der Datensatz wurde auf einem Areal aufge- 
nommen, welches neben urbanen Strukturen und Vegetation mit Büschen, 
Hecken und Bäumen auch Flächen mit geparkten Fahrzeugen beinhaltet. 
Außerdem befinden sich hohe Gebäude in der Nähe, was mit für GNSS 
kritischen Abschattungen und Mehrwegeausbreitungssituationen einher- 
geht. 

Die Integration der GNSS-Daten erlaubt auch eine Geo-Referenzie- 
rung und die Angabe der globalen Koordinaten erfolgt in dem Universal- 
Transverse-Mercator-System (UTM-System), welches heute sehr weit ver- 
breitet ist. Die hier dargestellte und alle folgenden Karten befinden sich 
in der UTM-Zone 32 und Band U. Zur besseren Vergleichbarkeit wurde 
bei den zum Vergleich herangezogenen Methoden und Konfigurationen 
nur mit Rad-Odometrie und 2D-LiDAR die Anfangspose auf die globale 
Startpose gesetzt. 


6.4.1 Evaluation 


Abbildung 6.16 zeigt das Kartierungsergebnis des modularen SLAM-Algo- 
rithmus unter Verwendung der Sensordaten von Rad-Odometrie und 2D- 
LiDAR mit nur einem Partikel. Im linken Teil der Karte ist ein großes 
kreuzförmiges Gebäude zu erkennen, während im rechten Teil Vegeta- 
tion mit Bäumen und Büschen vorherrscht. Das gesamte Gebiet ist durch 
einen Zaun mit einer dichten Hecke eingeschlossen und im unteren rech- 
ten Teil ist ein großer Parkplatz zu finden. Der befahrene Pfad beginnt ca. 
bei Koordinate (110m, 150 m) und geht zweimal im Gegenuhrzeigersinn 
um das große Gebäude, d.h., nach der ersten Runde ist eine große Schleife 
zu schließen. Nach der zweiten Runde führt der Pfad in mehreren Schlei- 
fen durch die Vegetation und anschließend über den Parkplatz, ebenfalls 


in mehreren Schleifen, um schließlich nahe dem Startpunkt zu enden. 
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Abbildung 6.16: Kartierungsergebnis des modularen SLAM-Algorithmus mit Rad- 
Odometrie und 2D-LiDAR mit einem Partikel. Der Pfad ist in Hellblau dargestellt und der 
Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Auch unter ausschließlicher Verwendung von Rad-Odometrie und 2D- 
LiDAR konnte eine nahezu konsistente Karte erzeugt werden, jedoch weist 
sie einige kleine Ungenauigkeiten auf, vor allem beim Schließen der großen 
Schleife ca. bei Koordinate (100m, 170m) sind mehrere Wände doppelt 
kartiert worden. Dieser Ausschnitt ist in Abbildung 6.17 nochmals in einer 
Detailansicht gezeigt. 

Der in Abschnitt 6.2 zur vergleichenden Evaluation herangezogene 
GMapping-Algorithmus nach [Gri07] lässt sich hier nicht anwenden, 
da er keine einfache Integration von zusätzlichen Sensoren wie GNSS 
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UTM-Hochwert - 5429097 inm 
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Abbildung 6.17: Detailausschnitt des Kartierungsergebnis des modularen SLAM- 
Algorithmus mit Rad-Odometrie und 2D-LiDAR mit einem Partikel. Der Pfad ist in Hellblau 
dargestellt und der Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


erlaubt. In Abschnitt 6.2 wurde außerdem gezeigt, dass mit dem modula- 
ren SLAM-Algorithmus gleichwertige Ergebnisse erzielt werden können. 
Mit Hector-SLAM wurde ein anderer etablierter SLAM-Algorithmus zum 
Vergleich gewählt, da dieser zwar keine Fusion mit einem zusätzlichen 
Sensor erlaubt, jedoch eine einfache Möglichkeit bietet, die Information 
eines globalen Sensors zu integrieren. Die Lokalisierung in Hector-SLAM 
wird über das Scan-Matching mit der Karte realisiert, wobei die Start- 
pose für das Scan-Matching aus einer zusätzlichen Bewegungsschätzung 
vorgegeben werden kann. Im Gegensatz zu den Ansätzen mit Partikel- 
filtern gibt es bei Hector-SLAM lediglich eine einzige Lokalisierung und 
Karte [Koh11]. Abbildung 6.18 zeigt das Ergebnis von Hector-SLAM, wobei 
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die Bewegungsschätzung für die Startpose des Scan-Matchings mittels Rad- 
Odometrie erfolgt. Das Schließen der ersten großen Schleife bei Position 
(200 m, 200 m) schlägt hier fehl und im weiteren Verlauf kann dies nicht 
mehr korrigiert werden, sondern führt auf der linken Seite bei Position 
(125 m, 175 m) letztendlich dazu, dass die Kartierung scheitert. 
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Abbildung 6.18: Kartierungsergebnis von Hector-SLAM mit Rad-Odometrie und 2D-LiDAR. 
Der Pfad ist in Hellblau dargestellt und der Grauwert bildet die Belegtheitswahrscheinlichkeit 
von 0 bis 1 ab. 


Abbildung 6.19 zeigt das Ergebnis des Hector-SLAM-Algorithmus, 
wenn fiir die Startposition des Scan-Matchings jeweils die aktuelle GNSS- 
Position und Ausrichtung aus der virtuellen Kompass-Messung mit ein- 
geht. D.h., hier geht nun eine absolute Pose in die Startpose fiir das Scan- 
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Abbildung 6.19: Kartierungsergebnis von Hector-SLAM mit Rad-Odometrie, GNSS und 2D- 
LiDAR. Der Pfad ist in Hellblau dargestellt und der Grauwert bildet die Belegtheitswahr- 
scheinlichkeit von 0 bis 1 ab. 


Matching ein, wodurch lokale Fehler korrigiert werden können und sich 
nicht mehr wie in Abbildung 6.18 katastrophal auswirken. Dennoch sind 
Ungenauigkeiten beim Schließen der ersten großen Schleife ca. bei Koor- 
dinate (100 m, 170 m) zu erkennen. 

Das Ergebnis des modularen SLAM-Algorithmus mit einem Partikel 
mit zusätzlich integrierten GNSS-Daten und Messungen des virtuellen 
Kompasses ist in Abbildung 6.20 dargestellt. Gegenüber dem Ergebnis des 
modularen SLAM-Algorithmus nur mit Rad-Odometrie und 2D-LiDAR ist 
eine Verbesserung deutlich zu erkennen. Die Karte weist keine Ungenau- 
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Abbildung 6.20: Kartierungsergebnis des modularen SLAM-Algorithmus mit Rad- 
Odometrie, GNSS und 2D-LiDAR mit einem Partikel. Der Pfad ist in Hellblau dargestellt 
und der Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


igkeiten mehr im oberen Teil auf. D.h. die erste große Schleife konnte 
mit verbesserter Genauigkeit geschlossen werden. Dieser Ausschnitt ist in 
Abbildung 6.21 nochmals in einer Detailansicht gezeigt. 

Abbildung 6.22 zeigt das Kartierungsergebnis des modularen SLAM- 
Algorithmus wieder ohne zusätzliche Integration von GNSS-Daten und 
Kompass-Messungen, jedoch mit einer Anzahl von 80 Partikeln. Auch hier 
konnte eine Karte erstellt werden, welche keine Ungenauigkeiten mehr 
aufweist. D. h., es konnten über die Diversitat der Partikel nach dem ersten 
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UTM-Hochwert - 5429097 inm 
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Abbildung 6.21: Detailansicht des Kartierungsergebnis des modularen SLAM-Algorithmus 
mit Rad-Odometrie, GNSS und 2D-LiDAR mit einem Partikel. Der Pfad ist in Hellblau darge- 
stellt und der Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Schleifenschluss die Partikel mit den Ungenauigkeiten in der jeweiligen 


Karte beim Resampling eliminiert werden. 


6.4.2 Resampling und Laufzeit 


Der modulare SLAM-Algorithmus setzt eine adaptive Resampling-Strategie 
ein, welche das Resampling nur ausführt, wenn die Partikel die Zielver- 
teilung nicht mehr gut approximieren, siehe auch Abschnitt 3.2.1. D. h., 
je besser die Zielverteilung approximiert wird, desto seltener wird das 


Resampling durchgeführt. 
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Abbildung 6.22: Kartierungsergebnis des modularen SLAM-Algorithmus mit Rad- 
Odometrie und 2D-LiDAR mit 80 Partikeln. Der Pfad ist in Hellblau dargestellt und der 
Grauwert bildet die Belegtheitswahrscheinlichkeit von 0 bis 1 ab. 


Der Datensatz hat insgesamt 5854 LiDAR-Messungen und zum Ver- 
gleich wurden zehn Durchläufe mit 100 Partikeln jeweils mit und ohne 
GNSS-Messungen durchgeführt. Bei dem modularen SLAM-Algorithmus 
mit lediglich Rad-Odometrie und 2D-LiDAR wurde bei diesem Datensatz 
zwischen 14 und 18 Mal (im Schnitt 16,8 Mal) Resampling durchgeführt. 
Mit zusätzlicher Integration der GNSS- und Kompass-Messungen waren 
es nur noch acht bis neun Mal (im Schnitt 8,3 Mal). Die höhere Anzahl 
von Resampling-Operationen, bei dem Partikel mit den zugehörigen Kar- 
ten kopiert werden müssen, wirkt sich auch auf die Laufzeit aus. Eben- 
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falls einen Einfluss auf die benötigte Rechenzeit hat die Lokalisierung in 
der Karte mittels Scan-Matching, welche bei einem besseren Startwert 
schneller konvergiert. So benötigte der modulare SLAM-Algorithmus mit 
lediglich Rad-Odometrie und 2D-LiDAR für den kompletten Datensatz auf 
einem Intel Xeon E5-2687W v2 mit 3,4GHz und acht Kernen im Schnitt 
7528 s und mit zusätzlicher Integration der GNSS-Messungen nur 6701 s. 
Für eine Analyse der Laufzeiten bei unterschiedlichen Anzahlen an 
Partikeln wurden weitere Durchläufe durchgeführt. Auch dieses Mal wurde 
wieder über jeweils zehn Durchläufe gemittelt, wobei nur ein Ausschnitt 
aus dem Datensatz ohne Schleifen verwendet wurde. Es kam ebenfalls 
ein Intel Xeon E5-2687W v2 mit 3,4 GHz und acht Kernen zum Einsatz. 
Die Ergebnisse sind in Tabelle 6.3 mit den jeweiligen Rechenzeiten pro 


Aktualisierungsschritt in ms aufgeführt. 


Tabelle 6.3: Mittlere Rechenzeit pro Aktualisierungsschritt in ms 


Partikelzahl 1 2 4 8 16 32 64 128 


Rechenzeitinms 29 54 58 69 127 239 459 907 


Da die Partikel parallel berechnet werden, wäre im Idealfall bei einem 
Prozessor mit acht Kernen mit bis zu acht Partikeln eine in etwa gleich- 
bleibende Laufzeit zu erwarten und darüber ein proportionaler Anstieg. 
Bei einem realen Prozessor spielen jedoch noch weitere Faktoren, wie z.B. 
Cache-Größe oder dynamische Taktanpassung, eine Rolle. Außerdem gibt 
es Teile in dem Partikelfilteralgorithmus, welche nicht parallelisierbar sind, 
wie z.B. die Berechnung des effektiven Partikelgewichts und das Resamp- 
ling. Von nur einem auf zwei Partikel ist ein Anstieg der Laufzeit zu beob- 
achten, da bei nur einem Partikel die genannten und weitere Berechnun- 
gen, wie beispielsweise die Berechnung der Gewichte der Partikel, nicht 
anfallen. Zwischen zwei und vier Partikeln ist die Laufzeit zwar nahezu 
gleichbleibend, bei acht Partikeln steigt die Laufzeit jedoch etwas an, was 
darauf zurückzuführen ist, dass der Prozessor über eine dynamische Anpas- 


sung der Taktfrequenz verfügt und bei Auslastung aller Kerne nicht mehr 
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mit dem maximalen Takt läuft. Ab acht Partikeln und mehr ist bei Ver- 
doppelung der Anzahl der Partikel beim Anstieg der Laufzeit in etwa eine 
Verdoppelung zu beobachten. Angesichts dessen, dass ein gewisser Teil 
nicht parallelisierbar ist, entspricht die Skalierung in etwa der theoretisch 
zu erwartenden Laufzeit proportional zur Anzahl der Partikel. 


6.4.3 Zusammenfassung 


Die Evaluation hat gezeigt, dass durch die Hinzunahme eines absoluten 
Sensors die Robustheit und Präzision von SLAM-Algorithmen hinsichtlich 
des Schließens großer Schleifen effektiv gesteigert werden kann. Durch 
die Fusion mit zusätzlichen GNSS-Messungen konnte bei dem modula- 
ren SLAM-Algorithmus selbst mit einem Partikel eine sehr genaue Karte 
erzeugt werden, wofür ohne die Integration der GNSS-Messungen 80 Par- 
tikel notwendig waren. Aber auch bei der gleichen Anzahl von Partikeln 
konnten Vorteile für die Integration des GNSS-Empfängers aufgezeigt wer- 
den. Das adaptive Resampling musste seltener durchgeführt werden, was 
bedeutet, dass die Zielverteilung besser approximiert wird. Dies führt einer- 
seits zu einer geringeren Rechenzeit und wirkt sich andererseits positiv 
auf die Diversität der Partikel aus, was letztendlich zu einer robusteren 
Kartierung führt. 

Des Weiteren wurde gezeigt, dass auch bei anderen Methoden wie 
Hector-SLAM die Integration der Information eines absoluten Sensors hin- 


sichtlich der Robustheit unterstützend wirken kann. 


6.5 SC-EKF 


Für die Evaluation der Filter mit stochastischem Klonen werden die neu- 
artigen Methoden des impliziten Klonens mit Smoothing nach Abschnitt 
5.2.4 und des partiellen impliziten Klonens mit Smoothing nach Abschnitt 
5.2.5 mit den bestehenden Methoden des stochastischen Klonens vergli- 


chen und Faktor-Graph-Methoden gegenübergestellt. 
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Der für die Evaluierung herangezogene Datensatz wurde wieder mit 
der in Abschnitt 6.2 erwähnten geländegängigen Plattform aufgenommen. 
Der Datensatz wurde außerdem wieder auf demselben Areal aufgenom- 
men, welches neben urbanen Strukturen und Vegetation mit Büschen, 
Hecken und Bäumen auch Flächen mit geparkten Fahrzeugen umfasst. 
Verwendet wurden die Sensordaten eines MTi-G-700 von Xsens, welches 
sowohl eine IMU als auch einen GNSS-Empfänger beinhaltet, sowie die 
Messungen eines Rad-Odometriesensors und eines 3D-LiDARs Velodyne 
HDL-64E. Der 3D-LiDAR hat 64 vertikal von —24,8° bis 2° fächerförmig 
angeordnete Laser und eine Drehrate vom 10 Hz, d.h., eine Umdrehung 
benötigt 100 ms. Währenddessen findet eine Bewegung des Fahrzeugs statt, 
was zu einer Verzerrung des Scans führt. Diese Verzerrung kann mit- 
tels inertialer Korrektur kompensiert werden, indem der Scan anhand der 
Eigenbewegungsschätzung des Filters entzerrt wird. Normalerweise wird 
diese Eigenbewegungsschätzung schritthaltend aus dem Filter herange- 
zogen. Für diese Evaluation wurde sie jedoch vorausberechnet, um eine 
bessere Vergleichbarkeit der Algorithmen zu gewährleisten. 

Die Sensordaten gehen wie in Abschnitt 5.3.1 beschrieben in das SC- 
EKF ein. Die IMU-Daten haben eine Rate von 100 Hz und werden im Prä- 
diktionsschritt und einem Korrekturschritt für die Lagekorrektur des Nick- 
und Rollwinkels verwendet. Nach der Lagekorrektur wird auch die nicht- 
holonome Korrektur angewendet. Die GNSS-Daten haben eine Rate von 
4Hz und gehen als absolute Korrektur für die 3D-Position und die linearen 
Geschwindigkeiten ein. Zeitgleich wird der Gierwinkel in einem Vorver- 
arbeitungsschritt aus den 2D-Positionsmessungen des GNSS geschätzt 
und geht als weitere Korrektur mit ein. Die Rad-Odometrie und der 3D- 
LiDAR haben beide eine Rate von 10 Hz und während die Rad-Odometrie 
als Messung der Vorwärtsgeschwindigkeit eingeht, wird aus dem 3D- 
LiDAR-Scan und dem vorhergehenden Scan mittels Scan-Matching eine 
6DoF-Differenzpose ermittelt. Diese 6DoF-Differenzpose wird als relative 
Messung per Korrektur mit stochastischem Klonen integriert. Für das 


Scan-Matching mittels GICP werden die Scans räumlich unterabgetastet, 
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da hierdurch der Rechenaufwand deutlich reduziert wird und gleichzei- 
tig die Genauigkeit kaum verringert wird. So kann die Berechnung des 
Scan-Matching mehr als zwölfmal so schnell erfolgen und die Abwei- 
chungen zum Scan-Matching mit dem vollen Scan betragen im Schnitt 
weniger als 3cm bzw. 0,06°. Wie bei der inertialen Korrektur wurde für 
eine bessere Vergleichbarkeit der Algorithmen die 6DoF-Differenzpose mit 
dem Scan-Matching für die aufeinanderfolgenden Scan-Paare vorberech- 
net. Das Scan-Matching beim Schließen von Schleifen wurde dagegen für 
jeden Ansatz individuell berechnet, da die Schleifenschlüsse auch jeweils 
unterschiedlich waren. 


6.5.1 Implizites Klonen 


In Abschnitt 5.2.1 wurde dargestellt, dass die Vereinfachung mit der Rekon- 
struktion der augmentierten Systemkovarianzmatrix mittels Zut zwar 
für eine Konfiguration mit zwei relativen Sensoren gültig ist, jedoch bei 
der Hinzunahme eines weiteren asynchronen Sensors zu einem instabilen 
Filterverhalten führen kann. 

Dies ist auch in der Evaluation nachweisbar, wie in Abbildung 6.23 
zu erkennen ist. Da die Messungen des GNSS nicht zeitgleich mit den 
relativen Korrekturen des 3D-LiDARs auftreten, können die Schätzungen, 
welche nur durch die relativen Messungen korrigiert werden, gegenüber 
den Schätzungen mit den absoluten Korrekturen wegdriften. Da die Kor- 
rektur durch das Scan-Matching mit den aufeinanderfolgenden Scans eine 
hohe Genauigkeit hat, führt dies zu einer hin- und herspringenden Schät- 
zung und letztendlich zu einem instabilen Filter. Wenn z. B. zum Zeitpunkt 
k +n eine GNSS-Messung als absolute Korrektur in das Filter eingeht, wird 
nur die aktuelle Lokalisierungsschätzung korrigiert. Eine darauf folgende 
relative Messung zum Zeitpunkt k + m mit m > n bezieht sich auf die 
Zustandsschätzung zum Zeitpunkt k, welcher nicht durch die absolute 
Korrektur zum Zeitpunkt k + n aktualisiert wurde. 


153 


6 Ergebnisse und Auswertung 


-5 T T T T T 


Geschätzter Pfad x 
xX GNSS 


UTM-Rechtswert 
- 458059 in m 


-1 fi 1 L fi L 
0 2 4 6 8 10 12 


UTM-Hochwert - 5429340 in m 


Abbildung 6.23: Divergierende Pfadschätzung mit implizitem Klonen. Der geschätzte Pfad 
ist in Hellblau dargestellt und die GNSS-Messungen als orange Kreuze. 


Abbildung 6.24 zeigt den gleichen Ausschnitt mit der in Abschnitt 5.2.4 
vorgestellten Methode mit implizitem Klonen mit Smoothing, welche 
jeweils vor der Korrektur durch die relative Messung durchgeführt wird. 
Durch das Smoothing wird die zum Zeitpunkt k + n durch die GNSS- 
Messung eingebrachte absolute Information zurück zu dem geklonten 
Zustand des Zeitpunkts k propagiert und dadurch die Drift vermindert 


und ein Divergieren des Filters unterbunden. 
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Abbildung 6.24: Pfadschätzung mit implizitem Klonen und Smoothing. Der geschätzte Pfad 
ist in Hellblau dargestellt und die GNSS-Messungen als orange Kreuze. 
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Hinsichtlich der Stabilität beim impliziten Klonen mit Smoothing 
konnte außerdem beobachtet werden, dass es zur Degeneration der Sys- 
temkovarianzmatrix kommen kann, wenn nicht das modifizierte kumu- 
lative Produkt Fk+m,k, sondern lediglich das ursprüngliche kumulative 
Produkt der Zustandsübergangsmatrizen Fx+m,x verwendet wird. Die 
Degeneration äußert sich durch negative Diagonalelemente, d. h., die Sys- 
temkovarianzmatrix ist dann nicht mehr positiv semidefinit. Dies trat 
in den Evaluationen allerdings nur auf, wenn für die Messunsicherheit 
des relativen Sensors extrem niedrige Werte eingesetzt wurden. Mit dem 
modifizierten kumulativen Produkt Fx+m,x konnten keinerlei Instabilitä- 


ten dieser Art beobachtet werden. 


6.5.2 Simulierter GNSS-Ausfall 


Da wie auch schon bei den Evaluierungen zuvor keine Ground-Truth- 
Daten von einem Referenzsystem oder Referenzmessungen vorhanden 
sind, wurde zum Vergleich der Methoden mit stochastischem Klonen ein 
simulierter GNSS-Ausfall herangezogen. Während des simulierten Aus- 
falls ist mit einer Verschlechterung der Posenschätzung zu rechnen, da 
keine absolute Position und kein Gierwinkel zur Stützung zur Verfügung 
steht. Hiermit kann die in Abschnitt 5.3.4 beschriebene Ausfallsicherheit 
gezeigt werden und die Methoden können hinsichtlich der auftretenden 
Verschlechterung miteinander verglichen werden. 

Bei der Evaluation wurde ein Ausfall des GNSS für ca. 38 s bzw. über 
eine Strecke von ca. 130m simuliert und die sich während des Ausfalls 
ergebene Drift im Vergleich zu den jeweilig gleichen Methoden ohne den 
Ausfall des GNSS analysiert. Folgende Methoden wurden für die Evaluation 


miteinander verglichen: 
1. Kein Scan-Matching, 


2. Implizites Klonen mit zusätzlicher Offset-Schätzung nach [Lyn13], 


3. Implizites Klonen mit Smoothing nach Abschnitt 5.2.4, 
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4. Partielles implizites Klonen mit Smoothing nach Abschnitt 5.2.5, 


5. Explizites Klonen nach Abschnitt 5.2.2, 


6. Partielles explizites Klonen nach Abschnitt 5.2.3. 


Dabei sind die im Folgenden verwendeten Bezeichnungen unterstrichen. 
Bei der ersten Methode werden keine relativen Messungen des 3D-LiDARs 
mit einbezogen, d.h., während des Ausfalls des GNSS stehen als Senso- 
ren nur noch die IMU und die Rad-Odometrie zur Verfügung. Dies stellt 
den ungünstigsten Fall dar und dient daher zur Orientierung. Die zweite 
Methode zeichnet sich dadurch aus, dass sie implizites Klonen einsetzt und 
die in Abschnitten 5.2.1 und 6.5.1 aufgezeigte Drift als 6DoF-Offset zur 
Kompensation mitschätzt, wodurch der Zustandsraum um sechs zusätzli- 
che Variablen erweitert wird [Lyn13]. Bei der dritten Methode handelt es 
sich um die neuartige in Abschnitt 5.2.4 vorgestellte Methode mit implizi- 
tem Klonen und Smoothing. Bei der vierten Methode handelt es sich um 
das partielle implizite Klonen nach Abschnitt 5.2.5, wobei es sich bei dem 
relevanten geklonten Teil um die 6DoF-Pose der Zustandsschätzung han- 
delt, da diese von der relativen 6DoF-Pose des Scan-Matchings betroffen 
ist. Die fünfte Methode setzt das in Abschnitt 5.2.2 vorgestellte explizite 
Klonen um. Bei der letzten Methode wird ebenfalls ein explizites Klonen 
umgesetzt, allerdings werden hier wieder nur die sechs Zustandsvariablen 
der 6DoF-Pose geklont, vgl. Abschnitt 5.2.3. Alle Methoden wurden im 
selben Filter-Framework umgesetzt und mit allen wurde der gleiche oben 
erwähnten Datensatz verarbeitet, um so eine adäquate Vergleichbarkeit 
sicherzustellen. 

Für die Evaluation der Methoden wird eine Fehlermetrik definiert, wel- 
che den euklidischen Abstand der geschätzten Position zu einer Referenz- 
position darstellt und im Folgenden als 3D-Positionsfehler bezeichnet wird: 


Cant = af (Xk — Xref,k)? + (Yk Meet El + (Zk — Zref,k)? > (6.1) 
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wobei die Komponenten der jeweiligen Referenzposition ohne GNSS- 
Ausfall mit dem Index „ref“ gekennzeichnet sind. Da in der Zustandsschät- 
zung die 2D-Position in Längen- und Breitengrad repräsentiert ist, wird 
sie in UTM-Koordinaten konvertiert, um zusammen mit der Höhe den 3D- 
Positionsfehler in kartesischen Koordinaten berechnen zu können. 
Abbildung 6.25 zeigt die 3D-Positionsfehler im Zeitraum um den GNSS- 
Ausfall. Der Zeitraum des GNSS-Ausfalls geht von 18 s bis 56 s und ist grau 
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Abbildung 6.25: 3D-Positionsfehler im Zeitraum um den GNSS-Ausfall. Der Zeitraum des 
GNSS-Ausfalls ist grau hinterlegt. 


hinterlegt. Vor dem GNSS-Ausfall ist der Fehler aller Varianten 0m, da 
sie gegen sich selbst verglichen werden. Sobald das GNSS ausfällt, steigen 
bei allen Varianten die Fehler mit ähnlicher Tendenz und werden erwar- 


tungsgemäß schnell wieder reduziert, wenn das GNSS wieder verfügbar 
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ist. Die Methode ohne Scan-Matching weist wie zu erwarten den größ- 
ten maximalen Fehler von über 7 m auf. Die Offset-Methode ist mit einem 
maximalen Fehler von um die 5 m besser, jedoch haben die Methoden mit 
explizitem Klonen und implizitem Klonen deutlich geringere maximale 
Fehler von unter 3m bzw. um die 2 m. Das Ergebnis der Methode des par- 
tiellen expliziten Klonens ist nicht in die Abbildung mit aufgenommen, da 
es eine maximale Differenz von weniger als 1mm zur Methode des kom- 
pletten expliziten Klonens aufweist, d.h., die in Abschnitt 5.2.3 diskutierten 
Einflüsse durch die Kreuzkovarianzen sind hier sehr gering. Während des 
GNSS-Ausfalls hat das partielle implizite Klonen eine maximale Differenz 
von 17 cm zum kompletten impliziten Klonen. Der Unterschied zwischen 
dem partiellen und dem kompletten impliziten Klonen ist im Vergleich 
zu den expliziten Varianten also deutlich größer, was auf die zusätzlichen 
Approximationen bei der Rekonstruktion der augmentierten Systemko- 
varianzmatrix beim partiellen impliziten Klonen zurückzuführen ist, vgl. 
Abschnitt 5.2.5. 

Abbildung 6.26 zeigt die Fehler des Gierwinkels y im Zeitraum um den 
GNSS-Ausfall. Die horizontalen Lagewinkel wurden nicht verglichen, da 
sie auch während des GNSS-Ausfalls über die Beschleunigungssensoren 
korrigiert werden. Vor dem GNSS-Ausfall ist der Fehler aller Varianten 
wieder 0°, da sie gegen sich selbst verglichen werden. Sobald der Ausfall 
einsetzt, sind für alle Varianten tendenziell ähnliche Verläufe mit unter- 
schiedlichen Amplituden zu beobachten. Wie bei der Position ergeben sich 
ohne Scan-Matching während des Ausfalls die größten maximalen Fehler 
von mehr als +10°. Die Offset-Methode ist mit einem maximalen Fehler 
von unter +7,5° etwas besser. Die Methoden mit explizitem Klonen und 
implizitem Klonen weisen nochmals deutlich geringere maximale Fehler 
von unter +2,5° auf. Zusammen mit dem Ergebnis des Vergleichs der 3D- 
Positionsfehler in Abbildung 6.25 ist festzustellen, dass die Methoden mit 
explizitem Klonen bzw. implizitem Klonen eine vergleichbare Genauigkeit 
erzielen. Das partielle explizite Klonen ist wiederum nicht in die Abbil- 


dung mitaufgenommen, da es eine maximale Differenz von weniger als 
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Abbildung 6.26: Fehler des Gierwinkels im Zeitraum um den GNSS-Ausfall. Der Zeitraum 
des GNSS-Ausfalls ist grau hinterlegt. 


0,01° zum kompletten expliziten Klonen aufweist. Das partielle implizite 
Klonen hat im Vergleich dazu wiederum eine höhere maximale Differenz 
von 0,17° zum kompletten impliziten Klonen. 

Der große Überschwinger beim Wiedereinsetzen des GNSS von mehr 
als 15° bei der Variante ohne Scan-Matching kann damit erklärt wer- 
den, dass zu diesem Zeitpunkt der Gierwinkel noch nicht durch abso- 
lute Messungen gestützt wird, da die vorgelagerte Gierwinkelschätzung 
mehrere GNSS-Messungen benötigt. Die IMU-Messungen liefern lediglich 
eine Information über die Orientierungsänderung und durch die nicht- 
holonome Korrektur wird der Gierwinkel deshalb in Richtung der GNSS- 


Messung gezogen und erst langsam korrigiert, sobald die Gierwinkelschät- 
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zung mittels GNSS-Messungen kurze Zeit später wieder zur Verfügung 
steht. Die Filtervarianten mit dem stochastischen Klonen haben noch 
zusätzlich die sehr genauen relativen Posen des Scan-Matching, welche 
stärkere Orientierungsänderungen verhindern. Außerdem fallen die not- 
wendigen Positionskorrekturen nicht so stark aus, da die Abweichungen 
zum Zeitpunkt des Wiedereinsetzens des GNSS nicht so groß sind wie 
ohne Scan-Matching, vgl. Abbildung 6.25. 


Zeitlicher Verlauf der Systemkovarianzen 


In diesem Abschnitt werden ausgesuchte zeitliche Verläufe von Elementen 
der Systemkovarianzmatrizen der unterschiedlichen Methoden miteinan- 


der verglichen. 
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Abbildung 6.27: Autokovarianzen des Hochwerts. 
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In Abbildung 6.27 sind die Autokovarianzen des Hochwerts y der ver- 
schiedenen Methoden im zeitlichen Verlauf ohne Ausfall des GNSS dar- 
gestellt und die Methoden mit explizitem Klonen und implizitem Klonen 
weisen nahezu identische Verläufe auf. Die Methode mit partiellem impli- 
ziten Klonen hat tendenziell auch einen sehr ähnlichen Verlauf, jedoch 
mit einem stets etwas abweichenden Wert, was darauf hindeutet, dass die 
Kreuzkovarianzen zu den nicht geklonten Zustandsvariablen einen nicht 
vernachlässigbaren Einfluss haben. Der Ansatz mit dem Offset zeigt deut- 
lichere Abweichungen zu den anderen Methoden, was daran liegt, dass 
die Lokalisierungsunsicherheit zum Teil auch in dem zusätzlichen 6DoF- 
Offset modelliert ist. Die anderen Autokovarianzen der Zustandsvariablen 
der Pose zeigen ähnliche Verhältnisse. 

Abbildung 6.28 zeigt die Kreuzkovarianzen zwischen Hochwert und 
Gierwinkel. Die Methoden mit explizitem Klonen und implizitem Klo- 
nen haben wiederum nahezu identische Verläufe, was zusammen mit der 
Betrachtung der Autokovarianzen in Abbildung 6.27 zeigt, dass das Zurück- 
propagieren der Information durch Smoothing beim impliziten Klonen 
nahezu äquivalente Ergebnisse erzielt, während die zusätzlichen Approxi- 
mationen beim partiellen impliziten Klonen wieder zu leichten Abweichun- 
gen führen. Der Ansatz mit dem Offset hingehen zeigt wieder deutlichere 
Abweichungen. 

In Abbildung 6.29 sind die Varianzen der 3D-Position im Zeitraum 
um den GNSS-Ausfall dargestellt. Zum Vergleich ist zusätzlich die Varianz 
der 3D-Position der Methode mit explizitem Klonen ohne GNSS-Ausfall 
abgebildet. Die Varianz der 3D-Position ist dabei die Varianz der Summe der 
Komponenten und wird über das Aufsummieren der jeweilige Auto- und 


Kreuzkovarianzen von Rechtswert x, Hochwert y und Höhe z berechnet: 


GE > of +2 H oi; mit i, j € {x,y,z}. (6.2) 
i 


i<j 


Es ist deutlich zu erkennen, wie die Varianz wahrend des Ausfalls bei allen 


Methoden wie erwartet stetig ansteigt, wahrend sich die Varianz ohne 
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Abbildung 6.28: Kreuzkovarianzen zwischen Hochwert und Gierwinkel. 


GNSS-Ausfall im Vergleich kaum ändert. Sobald das GNSS wieder verfüg- 
bar ist, werden die Unsicherheiten durch die absoluten Messungen in kur- 
zer Zeit wieder reduziert. Der stetige Anstieg während des Ausfalls wird 
durch die Integration des Scan-Matching als relativer Sensor durch stochas- 
tisches Klonen gewährleistet und die Abweichungen in Abbildung 6.25 sind 
dabei stets kleiner als die Quadratwurzel der jeweiligen Varianz, d. h., die 
Varianzen der Filter werden nicht zu optimistisch berechnet. So können 
die absoluten Messungen des GNSS bei Verfügbarkeit auch die Schätzung 
in kurzer Zeit korrigieren, wie in Abbildungen 6.25 und 6.26 gezeigt wurde. 
Bei einer Modellierung der relativen Messungen beispielsweise als pseudo- 
absolute Messungen wäre die Varianz nicht angestiegen. D. h., die Messun- 
gen des GNSS hätten bei Wiederverfügbarkeit einen nur geringen Einfluss 
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Abbildung 6.29: Varianzen der 3D-Position im Zeitraum um den GNSS-Ausfall. Der Zeit- 
raum des GNSS-Ausfalls ist grau hinterlegt. 


gehabt und somit hätte die Korrektur der Schätzung sehr viel länger gedau- 


ert. 


Qualitativer Vergleich 


Wie bereits in Abschnitt 5.3.1 findet bei der Einbeziehung des 3D-LiDARs 
als relative 6DoF-Bewegung über Scan-Matching keine explizite Kartie- 
rung statt. Für einen qualitativen Vergleich anhand von erstellten Kar- 
ten können diese dennoch aufgezeichnet werden, ohne sie jedoch für 
die Lokalisierung zu verwenden. Als Kartenformat kommt hier die in 
Abschnitt 5.3.2 vorgestellte NDT-Karte mit inkrementeller Aktualisierung 


zum Einsatz. Während für das Scan-Matching zur Lokalisierung unterab- 
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getastete Scans Verwendung finden, werden für die Kartierung die vollen 
Scans verwendet. 


Abbildung 6.30: Beispiel einer NDT-Karte. Die Höhe ist farbkodiert von blau (niedrig) über 
grün zu rot (hoch). 


Abbildung 6.30 zeigt ein Beispiel für eine NDT-Karte einer gemisch- 
ten Umgebung. Auf der linken Seite befinden sich Gebäudestrukturen, bei 
denen Ellipsoide vorherrschen, welche flach und vertikal ausgerichtet sind. 
In der Mitte ist eine Straße mit Ellipsoiden, welche wie zu erwarten flach 
und horizontal ausgerichtet sind. Auf der rechten Seite ist ein unstruktu- 
rierter Bereich mit Vegetation in Form von Bäumen und Büschen, erkenn- 
bar an unregelmäßig verteilten Ellipsoiden, welche in alle Richtungen ähn- 
liche Ausdehnungen ohne Vorzugsrichtung aufweisen, vgl. Abschnitt 3.2.4. 

Abbildung 6.31a zeigt einen Ausschnitt aus der Karte, welche mit expli- 
zitem Klonen ohne GNSS-Ausfall aufgenommen wurde. Diese dient als 
Referenzkarte, da hier alle zur Verfügung stehenden Sensorinformatio- 
nen eingegangen sind. In der Mitte des Ausschnitts steht ein Block von 
Garagen, auf der linken Seite befindet sich eine lange gerade Hecke und 
auf der rechten Seite sind Teile eines großen Gebäudes sichtbar. In den 
anderen Karten ist der gleiche Ausschnitt für die verschiedenen Metho- 
den mit stochastischem Klonen bei GNSS-Ausfall gegenübergestellt. Abbil- 
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Abbildung 6.31: Kartierungsergebnis ohne GNSS-Ausfall mit explizitem Klonen (a), bei 
GNSS-Ausfall mit Offset (b), bei GNSS-Ausfall mit implizitem Klonen (c), bei GNSS-Ausfall 
mit explizitem Klonen (d). Die Höhe ist farbkodiert von blau (niedrig) über grün zu rot (hoch). 
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dung 6.31b zeigt das Kartierungsergebnis der Methode mit Offset und es 
sind deutliche Ungenauigkeiten an den Garagen und der Hecke in Form 
von Verdrehungen zu erkennen. In den Abbildungen 6.31c und 6.31d, den 
Kartierungsergebnissen mit implizitem Klonen bzw. explizitem Klonen, 
sind zwar auch Ungenauigkeiten an den gleichen Stellen zu erkennen, sie 
sind jedoch wesentlich weniger ausgeprägt. Diese Beobachtungen decken 
sich auch mit den Ergebnissen in den Abbildungen 6.25 und 6.26, bei denen 
die Methode mit Offset ebenfalls höhere Fehler aufweist. 


6.5.3 Vergleich mit Faktor-Graph-SLAM 


Um trotz der fehlenden Referenzlokalisierung eine Vergleichsmöglich- 
keit gegenüber einer einzigen Referenz zu realisieren, wurde ein SLAM- 
Algorithmus auf Basis eines Faktor-Graphen implementiert. Faktor-Graph- 
Methoden gehören momentan zu den leistungsfähigsten SLAM-Algorith- 
men, haben jedoch den Nachteil, dass sie sich nur bedingt für Echtzeit- 
anwendungen eignen, siehe Abschnitt 2.2.1. Bei den folgenden Untersu- 
chungen ist auch die Methode für das Schließen von Schleifen Teil der 
Evaluation. Da beim Schließen von Schleifen das Scan-Matching auch 
mit zurückliegenden Scans durchgeführt wird, müssen diese auch gespei- 
chert werden. Für das Scan-Matching zur Lokalisierung finden jedoch nur 
unterabgetastete Scans Verwendung, weshalb im realen Einsatz nur diese 
und nicht die vollen Scans abgespeichert werden müssten, da letztere nur 
für die Kartierung Verwendung finden, welche inkrementell erfolgt, vgl. 
Abschnitt 5.3.2. 

Der Faktor-Graph-SLAM-Algorithmus wurde mit dem GTSAM-Fra- 
mework implementiert. Dieses bietet eine flexible Faktor-Graph-Struktur, 
einige vordefinierte Faktoren für die Lokalisierung und Kartierung und 
verschiedene Optimierungsalgorithmen [GTSAM]. Zusätzlich bietet das 
Framework noch einen speziellen Faktor für die Integration von IMU- 
Messungen. Da die IMU-Messungen mit einer hohen Rate zur Verfügung 


stehen, werden mehrere aufeinanderfolgende Messungen in einem IMU- 
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Faktor vorab integriert, damit die Anzahl der benötigten Knoten und 
damit auch der Rechenaufwand reduziert wird [Car14; For15]. Wie auch in 
Abschnitt 5.3.4 erläutert, kann die hohe Rate der IMU im Vergleich zu den 
anderen Sensoren zudem für die Synchronisierung verwendet werden. Die 
Vorab-Integration mit dem IMU-Faktor wird immer bis zur nächsten Mes- 
sung eines beliebigen anderen Sensors durchgeführt. Da diese Messung 
meist nicht exakt auf die IMU-Messungen fällt, wird die letzte Vorab- 
Integration extrapoliert und das Integrationsintervall der nachfolgenden 
IMU-Messung entsprechend verkürzt. Die Messungen des GNSS gehen 
als Positions- und als Geschwindigkeitsfaktor ein, die Rad-Odometrie als 
2D-Zwischenfaktor. Die 3D-LiDAR-Messungen gehen tiber Scan-Matching 
sowohl zwischen aufeinanderfolgenden Posen als auch für das Schließen 
von Schleifen als 3D-Zwischenfaktor mit ein, d. h., auch hier findet keine 
explizite Kartierung statt [Emt18a]. 

Zusätzlich zur Graph-Optimierung über den gesamten Pfad-Posterior 
ist es mit GTSAM auch möglich, jederzeit eine Schätzung mit den bisher 
verfügbaren Informationen durchzuführen. Daher wurden mit dem Faktor- 
Graph zusätzlich zwei inkrementelle Lokalisierungslösungen berechnet 
und in den Vergleich mit aufgenommen. Eine der Lösungen wird mit- 
tels einer Levenberg-Marquardt-Optimierung berechnet. Diese wird auch 
für die Berechnung der Referenz über den gesamten Graphen verwendet 
und muss für jede Zwischenlösung erneut ausgeführt werden. Die zweite 
Lösung wird mit dem iSAM2-Algorithmus berechnet, welcher inkremen- 
telle Aktualisierungen durchführt, d. h. Lösungen von vorherigen Durch- 
läufen weiter nutzen kann und im Idealfall nur kleine Teile des Graphen 
optimiert [Kae12]. 

Wie bereits in der Einleitung des Abschnitts 6.5 erwähnt, wurde 
der Datensatz in einem Mischgebiet aufgenommen. Der abgefahrene 
Pfad hat eine Länge von ca. 1,8km, deckt sowohl urbane Strukturen als 
auch unstrukturierte Bereiche mit Vegetation ab und beinhaltet mehrere 
größere und kleinere Schleifen. Das Ergebnis des Faktor-Graph-SLAM- 


Algorithmus mit einer Optimierung über den vollen Pfad-Posterior ist in 
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Abbildung 6.32 dargestellt. Da die Erstellung der Karte hier erst mittels des 
finalen Pfad-Posteriors durchgeführt wird, sind für die Referenzkarte alle 
Scans zu speichern und es genügt nicht, wie bei den Filtern nur die unterab- 
getasteten Scans zu speichern. Die mit den optimierten Posenschätzungen 
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Abbildung 6.32: Kartierungsergebnis des Faktor-Graph-SLAM-Algorithmus mit einer Opti- 
mierung über den vollen Pfad-Posterior. Die Karte ist über ein Satellitenbild gelegt und der 
Pfad ist in Hellblau dargestellt. Die Höhe ist farbkodiert von blau (niedrig) über grün zu rot 
(hoch). 


erzeugte NDT-Karte ist in der Abbildung über ein Satellitenbild gelegt und 
der Pfad ist in Hellblau dargestellt. Dieser Pfad und die Karte dienen im Fol- 
genden als Referenz für die Evaluierung der unterschiedlichen Methoden 


mit stochastischem Klonen. 
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Für eine übersichtlichere Darstellung werden die unterschiedlichen 
Methoden mit stochastischem Klonen in den kommenden Evaluationen fol- 
gendermaßen abgekürzt: Offset (O), explizites Klonen (EK), partielles expli- 
zites Klonen (PEK), implizites Klonen mit Smoothing (IK) und partielles 
implizites Klonen mit Smoothing (PIK), mit (mS) oder ohne dem Schließen 
von Schleifen nach Abschnitt 5.3.3. Die Abkürzungen für die inkremen- 
tellen Faktor-Graph-Methoden sind Levenberg-Marquardt-Optimierung 
(FGLM) und iSAM2 (FGISAM), wobei diese beiden immer inklusive des 
Schließens von Schleifen sind. 

Abbildung 6.33 zeigt die 3D-Positionsfehler aller Methoden mit stochas- 
tischem Klonen und der beiden inkrementellen Faktor-Graph-Methoden 
bezogen auf den global optimierten Faktor-Graphen. Die Zeitpunkte, an 
denen Schleifen zu schließen sind, werden durch vertikale gestrichelte 
Linien angezeigt. Die partiellen Methoden (PEK und PIK) wurden nicht in 
die Abbildungen übernommen, da sie nur wenige Millimeter bzw. Zenti- 
meter Differenzen zu den jeweiligen Methoden mit komplettem Klonen 
aufwiesen. Die Schätzung des GNSS-Offsets nach Abschnitt 5.3.3 bei den 
Ansätzen mit stochastischem Klonen wird erst nach dem ersten Schleifen- 
schluss aktiviert, d.h., die Ergebnisse der Paare mit und ohne Schleifen- 
schluss sind davor identisch. Vor dem ersten Schleifenschluss weisen alle 
Methoden sehr hohe Fehler auf, insbesondere die Faktor-Graph-Methoden. 
Letzteres deutet darauf hin, dass die Faktor-Graph-Methoden mehr Ver- 
knüpfungen benötigen, um besser zu konvergieren. Nach dem Schleifen- 
schluss sind bei allen Methoden die Fehler unter 4m und zwischen den 
Methoden mit stochastischem Klonen ist kein signifikanter Unterschied zu 
erkennen. Es zeigen sich zwar etwas andere Verläufe, aber keine Vorteile 
durch die Hinzunahme des GNSS-Offsets für das Schließen von Schleifen. 
Dies ist auch nicht unbedingt zu erwarten, da bei diesem Ansatz der GNSS- 
Offset die Lokalisierung der späteren Durchläufe zu der Lokalisierung der 
ersten Schleife hinzieht und nicht wie bei den Faktor-Graph-Methoden die 
Korrektur auch rückwirkend erfolgt. D. h., wenn die erste Schleife einen 


größeren Fehler gegenüber der Referenz aufweist, zeigt sich mit dieser 
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Abbildung 6.33: 3D-Positionsfehler bezogen auf den global optimierten Faktor-Graphen. 


Fehlermetrik im späteren Verlauf ein höherer Fehler. Der Fehler der FGLM- 
Methode geht am Ende auf 0, da der gleiche Optimierungsalgorithmus wie 
bei der Referenz zum Einsatz kommt. Bei der FGISAM-Methode und den 
Filtern mit stochastischem Klonen ist am Ende ein Fehler von etwa Im zu 
beobachten. 

Da in Abbildung 6.33 die Unterschiede zwischen den Methoden nicht 
direkt sichtbar sind, ist in Abbildung 6.34 eine Violinengrafik des 3D-Posi- 
tionsfehlers gezeigt. Bei der Violinengrafik handelt es sich um eine Erwei- 
terung der Kastengrafik, welche zusätzlich zu den Quartilen (schwarzes 
Rechteck) und dem Median (weißer Balken) ein geglättetes Histogramm 
als Dichterepräsentation enthält [Hin98]. Hier lässt sich erkennen, dass 
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Abbildung 6.34: Violinengrafik des 3D-Positionsfehlers. 


sich die Dichten der 3D-Positionsfehler der Methoden mit explizitem (EK) 
und implizitem Klonen (IK) ohne Schleifenschluss kaum unterscheiden, 
wohingegen die Methode mit dem Offset (O) hier eine etwas andere Form 
aufweist. Mit Schleifenschluss ändern sich die Formen der Dichten bei den 
Methoden mit explizitem (EKmS) und implizitem Klonen (IKmS) und es ist 
ein leichter Unterschied zwischen beiden zu erkennen. Die Methode mit 
dem Offset (OmS) ändert sich hingegen mit dem Schleifenschluss kaum und 
unterscheidet sich daher deutlich von den anderen Methoden. Allgemein 
zeigt sich auch, dass hier mit dem Schleifenschluss keine Verbesserung 
erzielt wurde, vor allem bei der Methode mit dem Offset ist kaum eine 
Veränderung zu erkennen. Beide inkrementellen Faktor-Graph-Methoden 


weisen gegenüber den Filtern wesentlich höhere Ausreißer auf, der Haupt- 
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teil der 3D-Positionsfehler ist jedoch hin zu kleineren Werten verschoben 


und sie weisen geringere Medianwerte auf. 
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Abbildung 6.35: Änderung des 3D-Positionsfehlers. 


Für die Lokalisierung kann neben der absoluten Genauigkeit auch die 
Glattheit eine bevorzugte Eigenschaft sein, d. h., die Lokalisierung sollte 
möglichst frei von Sprüngen bzw. Diskontinuitäten sein. Hierfür wurden 
die Änderungen der 3D-Positionsfehler evaluiert und in Abbildung 6.35 
dargestellt. Die Zeitpunkte, an denen Schleifen zu schließen sind, werden 
wieder durch vertikale gestrichelte Linien angezeigt. Die Fehler der Filter 
mit stochastischem Klonen ändern sich in einem Zeitschritt nicht mehr 
als +15 cm, bei den Faktor-Graph-Methoden hingegen sind einzelne Spit- 


zen bis zu mehreren Metern zu beobachten. Diese Spitzen treten meist 
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beim Schließen von Schleifen auf, was darauf zurückzuführen ist, dass 
zu diesen Zeitpunkten zusätzliche Bedingungen in den Graphen einge- 
fügt werden, die große Bereiche des Graphen und damit die Gesamtlösung 
beeinflussen. Das Auftreten größerer Sprünge bei inkrementellen Faktor- 
Graph-Methoden konnte auch in anderen Veröffentlichungen beobachtet 
werden [Wil14; Emt18a]. 
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Abbildung 6.36: Gierwinkelfehler. 


In Abbildung 6.36 sind die Gierwinkelfehler dargestellt. Auch hier sind 
die partiellen Methoden PEK und PIK nicht mit abgebildet, da sie ledig- 
lich Unterschiede kleiner 0,003° bzw. 0,1° zu den jeweiligen Methoden mit 
den kompletten Klonen aufwiesen. Die Filter-Methoden haben bis auf die 
Offset-Methode (O) einen nahezu gleichen Verlauf mit Fehlern unter +4°. 
Die inkrementellen Faktor-Graph-Methoden weisen vor dem ersten Schlei- 
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fenschluss ein paar Ausreißer auf und sind danach ebenfalls unter +4°. 
Lediglich die Offset-Methode zeigt durchweg sehr hohe Fehler bis über 
+8°. Generell ist hier erkennbar, dass die Fehler der inkrementellen Faktor- 
Graph-Methoden bis auf den Anfang und einzelne Sprünge beim Schließen 
von Schleifen einen glatteren Verlauf aufweisen als die Filter-Methoden, 
was auf einen glättenden Effekt der Vorab-Integration der IMU-Messungen 
in dem speziellen IMU-Faktor schließen lässt. 

Die Präzision von GNSS-Messungen ist in der Höhe i. A. schlechter 
als in der horizontalen Ebene [Kou17]. Während die Strecke, welche bei 
der Aufnahme des Datensatzes abgefahren wurde, nahezu flach ist und 
nur einen Innenhof beinhaltet, welcher etwa 2m niedriger liegt, war in 
den GNSS-Messungen eine Differenz zwischen dem höchsten und dem 
niedrigsten Wert von 11,5 m zu beobachten. Die Lokalisierungslösung der 
Referenz weist eine Differenz von 2,4m in der Höhe auf, was sehr gut 
mit der Realität übereinstimmt, während die Filter und die inkrementel- 
len Faktor-Graph-Methoden eine Differenz von mehr als 10 m aufweisen. 
Hier zeigt sich im Vergleich ein Vorteil durch die Integration von Schlei- 
fenschlüssen bei den Filtern, welche mit Schleifenschluss durchweg gerin- 
gere Differenzen zwischen der minimalen und maximalen Höhe aufweisen, 
siehe Tabelle 6.4. Dabei fallen die Verbesserungen bei den Methoden mit 
explizitem bzw. implizitem Klonen mit Smoothing wesentlich deutlicher 
aus als bei der Methode mit Offset. 


Tabelle 6.4: Maximale Höhendifferenz in m. 


Ohne Schleifenschluss mit Schleifenschluss (mS) 


EK 12,1 10,4 
PEK 12,1 10,4 
IK 12,1 10,5 
PIK 12,1 10,5 
O 12,1 11,6 
FGISAM - 14,3 


FGLM = 11,8 
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Abbildung 6.37: 2D-Positionsfehler. 


Da beim GNSS in der Höhe sehr große Fehler auftreten und sich diese 
auch auf die Lokalisierungsschätzungen auswirken, wird zusätzlich die 
horizontale Abweichung, d. h. der 2D-Positionsfehler ep, ausgewertet und 
in Abbildung 6.37 dargestellt. Die Schleifenschlüsse sind wieder durch ver- 
tikale gestrichelte Linien dargestellt. Die 2D-Positionsfehler sind erwar- 
tungsgemäß wesentlich geringer als die 3D-Positionsfehler. Sie liegen größ- 
tenteils unter 2,5 m mit ein paar Spitzen bis ca. 3,5 m. 

In Tabelle 6.5 sind alle mittleren und maximalen 2D- und 3D-Positi- 
onsfehler zum Vergleich aufgelistet. Die mittleren 2D-Positionsfehler sind 
durchweg weniger als halb so groß wie die mittleren 3D-Positionsfehler. 
Die Positionsfehler der unterschiedlichen Filtermethoden unterscheiden 
sich nur geringfügig, im Vergleich zu den inkrementellen Faktor-Graph- 
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Methoden ergeben sich jedoch einige Unterschiede. Die maximalen 3D- 
Positionsfehler der Faktor-Graph-Methoden, vor allem der FGLM-Lösung, 
sind viel höher als die der Filtermethoden, wohingegen die mittleren 3D- 


Positionsfehler ähnlich sind. 


Tabelle 6.5: Mittlerer und maximaler 3D-Positionsfehler in m. 


€3p max(é3p) Ep max(é2p) 


EK 2,15 7,62 0,97 3,47 
EKmS 2,24 7,62 1,03 3,53 
PEK 2,15 7,61 0.97 3,47 
PEKmS 2,24 7,61 1,03 3,53 
IK 2,15 7,62 0,98 3,45 
IKmS 2,21 7,62 1,02 3,47 
PIK 2,16 7,62 0,99 3,42 
PIKmS 2,20 7,62 0,98 3,47 
O 2,18 7,62 1,03 3,64 
OmS 2,31 7,62 1,03 3,62 
FGISAM 2,50 13,29 0,67 3,01 


FGLM 1,96 9,51 0,80 3,03 


Wie bereits angesprochen, ist nicht zu erwarten, dass das Schließen 
von Schleifen durch einen GNSS-Offset den absoluten Fehler in der Loka- 
lisierung verringert, da die Lokalisierung in späteren Durchläufen zu der 
Lokalisierung der ersten Schleife hingezogen wird. Es hat sich jedoch 
gezeigt, dass durch die Integration von Schleifenschlüssen eine Reduktion 
der maximalen Höhendifferenzen erreicht wird, siehe Tabelle 6.4. Deshalb 
wurde noch eine Evaluation mit einer alternativen Fehlermetrik durchge- 
führt, welche von den Einzelkomponenten des 3D-Positionsfehler jeweils 
deren mittleren Fehler abzieht, um den Einfluss von etwaig größeren Feh- 
lern während der ersten Schleife zu verringern. Der resultierende relative 


3D-Positionsfehler hat eine im Vergleich zu Gleichung (6.1) modifizierte 
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Berechnung: 
€sp,k = 4] (Xk — Xref,k — Ex)? + (Yk — Yref,k — Cy)? + (Zk — Zref,k — ez)’, 
(6.3) 
mit 
č 
ex = K 3 Lo = Xref,k) > (6.4) 
k=1 
a 
Ey = Dk = Yet) (6.5) 
k=1 
e 
ez = K Sie = Zref,k) - (6.6) 
k=1 


Abbildung 6.38 zeigt die Violinengrafik für den relativen 3D-Positions- 
fehler. Im Gegensatz zu Abbildung 6.34 zeigt sich hier für das explizite 
(EKmS) und implizite stochastische Klonen (IKmS) eine deutliche Ver- 
besserung durch den Schleifenschluss. Außerdem sind die Formen bei 
diesen Ansätzen wieder sehr ähnlich. Bei der Methode mit dem Offset 
(OmS) ergibt sich auch in dieser Betrachtung keine Verbesserung durch 
den Schleifenschluss. Mit dem Schleifenschluss mittels GNSS-Offset sind 
bei diesem Ansatz zwei Offsets zu schätzen, was hier dazu führt, dass sich 
die Korrektur durch den Schleifenschluss zu gering auswirkt. Die inkre- 
mentellen Faktor-Graph-Methoden zeigen gegenüber Abbildung 6.34 nur 
leichte Verbesserungen, da diese bei der Aktualisierung auch vergangene 
Schätzungen verbessern und damit auch der Schleifenschluss mit verbes- 
serten Schätzungen erfolgt, was bereits den absoluten 3D-Positionsfehler 
reduziert. 

In Tabelle 6.6 sind die mittleren relativen 3D-Positionsfehler ohne und 
mit Schleifenschluss gegenübergestellt und auch hier ist außer für die 
Methode mit dem Offset (O) für alle Filtermethoden eine deutliche Ver- 


besserung mit dem Schließen von Schleifen zu beobachten. 
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Abbildung 6.38: Violinengrafik des relativen 3D-Positionsfehlers. 


In Tabelle 6.7 sind die Mediane der relativen 3D-Positionsfehler ohne 
und mit Schleifenschluss gegenübergestellt und hier ist ebenso außer für 
die Methode mit dem Offset (O) für alle Filtermethoden eine deutliche 
Verbesserung mit dem Schließen von Schleifen zu beobachten. Die Fil- 
termethoden ohne Offset weisen dabei mit Schleifenschluss eine mit den 
Faktor-Graph-Methoden vergleichbare Präzision auf. 


Qualitativer Vergleich 


Eines der Entwicklungsziele der Filter-Algorithmen ist auch die simultane 
Kartierung, wofür der GNSS-Offset nach Abschnitt 5.3.3 eingeführt wurde. 
In den vorherigen Betrachtungen wurden zwar keine besseren Ergebnisse 


mit dem Schließen von Schleifen für die absolute Genauigkeit der Lokali- 
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Tabelle 6.6: Mittlerer relativer 3D-Positionsfehler in m. 


Ohne Schleifenschluss mit Schleifenschluss (mS) 


EK 2,15 1,62 
PEK 2,15 1,62 
IK 2,15 1,63 
PIK 2,15 1,64 
O 2,17 2,27 
FGISAM ž 2,19 


FGLM - 1,66 
Tabelle 6.7: Median des relativen 3D-Positionsfehler in m. 


Ohne Schleifenschluss mit Schleifenschluss (mS) 


EK 2,04 1,54 
PEK 2,07 1,54 
IK 2,05 1,59 
PIK 2,07 1,60 
O 2,09 2,18 
FGISAM > 1,70 


FGLM 7 1,52 


sierung erzielt (siehe Tabelle 6.5), jedoch waren bei den Ergebnissen der 
relativen Betrachtung und bei der Differenz der maximalen und minimalen 
Höhe der Lokalisierung geringere Werte für die Fehlermaße zu beobachten, 
siehe Tabelle 6.6 bzw. 6.4. 

Wie in Abschnitt 6.5.2 sollen deshalb auch Karten zu einem qualitativen 
Vergleich herangezogen werden. Diese sind wieder NDT-Karten, welche 
mit den berechneten Schätzungen aufgezeichnet wurden, ohne sie jedoch 
für die Lokalisierung zu verwenden. Die Karten in Abbildung 6.39 zeigen 
den nord-westlichen Teil des Areals, wobei Abbildung 6.39a den Ausschnitt 
der Referenzkarte aus Abbildung 6.32 zeigt. In Abbildung 6.39b ist das Kar- 
tierungsergebnis mit der inkrementellen Faktor-Graph-Methode FGISAM 
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(d) 


Abbildung 6.39: Ausschnitt aus Kartierungsergebnis des Faktor-Graph-SLAM-Algorithmus 
mit einer Optimierung über den vollen Pfad-Posterior. (a), mit FGISAM (b), EKmS (c), EK (d). 
Die Höhe ist farbkodiert von blau (niedrig) über grün zu rot (hoch). 
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dargestellt. Es zeigt gegenüber der Referenz leichte Ungenauigkeiten auf 
der Straße in der Kurve. Die Hecke an dem linken und dem oberen Rand 
und die Gebäudewände am rechten und am unteren Rand sind dagegen 
sehr genau kartiert worden. Die Kartierungsergebnisse des Filters mit expli- 
zitem Klonen mit dem Schließen von Schleifen (EKmS) in Abbildung 6.39c 
bzw. ohne das Schließen von Schleifen (EK) in Abbildung 6.39d zeigen 
demgegenüber einige Ungenauigkeiten an den Gebäudewänden. Ansons- 
ten zeigt das Filter mit dem Schließen von Schleifen (EKmS) eine ähn- 
lich gute Karte wie die inkrementelle Faktor-Graph-Methode mit leichten 
Ungenauigkeiten auf der Straße, wenn auch an anderer Stelle. Das Filter 
ohne das Schließen von Schleifen (EK) zeigt hingegen sogar Inkonsistenzen 
bei der Straße, da diese zweimal in verschiedenen Höhen kartiert wurde 
(dunkelblau und hellblau). Dies liegt an den großen Fehlern in der Höhen- 
schätzung, welche hauptsächlich durch die fehlerbehafteten Messungen 
des GNSS verursacht werden. Sie konnten durch die zusätzliche Schätzung 
des GNSS-Offsets in Abbildung 6.39c verringert werden, sodass nur noch 


leichte Ungenauigkeiten in der Straße sichtbar sind. 


6.5.4 Rechenzeit 


Da das Filter auch fiir die echtzeitfahige Lokalisierung und Kartierung auf 
autonomen mobilen Robotersystemen mit begrenzten Ressourcen einge- 
setzt werden soll, sind die benötigten Rechenzeiten eine wichtige Eigen- 
schaft. 

Die mittleren Berechnungszeiten fiir die Integration der einzelnen 
Messungen für den Vergleich der verschiedenen Filter-Methoden sind in 
Tabelle 6.8 aufgelistet. Die Ergebnisse wurden auf einem Intel i7-7600U 
Mobilprozessor mit zwei Kernen und 2,8 GHz Basistakt erzielt. 

Die Messungen der IMU werden sowohl fiir den Strapdown-Algorith- 
mus der Prädiktion als auch die Korrektur der horizontalen Lage verwen- 
det. Zusätzlich wird bei der Integration der IMU-Messungen auch die 


Korrektur durch das nicht-holonome Modell angewendet. Da hier meh- 
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Tabelle 6.8: Mittlere Rechenzeit in us. 


EK PEK IK PIK O 


IMU @100Hz 115,1 715 53,7 51,4 99,7 
GNSS @ 4Hz 98,5 56,4 38,3 36,4 59,8 
Rad-Odometrie @ 10Hz 37,3 21,7 11,8 10,8 25,9 
LiDAR @ 10Hz 26,5 31,5 329,9 99,0 112,7 


rere Schritte durchgeführt werden, wird mehr Rechenzeit im Vergleich 
zur Integration von GNSS und Rad-Odometrie benötigt. Die Integration 
der GNSS-Messungen beinhaltet neben den Positions- und Geschwindig- 
keitskorrekturen den Vorverarbeitungsschritt der Gierwinkelschätzung 
und dessen Korrektur. Für die Integration der Rad-Odometrie ist ledig- 
lich ein Korrekturschritt im Filter nötig, weshalb sie wesentlich weniger 
Rechenzeit als die Integration der GNSS-Messungen benötigt. Die Integra- 
tion der relativen Messung des Scan-Matchings mit den 3D-LiDAR-Daten 
ist für die verschiedenen Ansätze unterschiedlich. Der Korrekturschritt 
des 3D-LiDARs bei der Offset-Methode (O) beinhaltet die Akkumulation 
von Fx+m,k, die Rekonstruktion der augmentierten Systemkovarianzma- 
trix Primi k+m-1 Und den eigentlichen Korrekturschritt des Kalman-Filters. 
Beim impliziten Klonen (IK) und partiellen impliziten Klonen (PIK) sind 
die gleichen Berechnungen notwendig, es kommt hier jedoch noch zu 
Beginn das Smoothing hinzu. Bei den beiden Ansätzen mit explizitem Klo- 
nen, sei dies komplett (EK) oder nur partiell (PEK), muss nur der Kalman- 
Korrekturschritt ausgeführt werden. Im Anschluss an den Korrekturschritt 
der 3D-LiDAR-Messungen wird wieder jeweils der alte Klon durch einen 
neu erzeugten ersetzt. 

Die Aktualisierungen mit den Messungen der IMU, des GNSS und der 
Rad-Odometrie benötigen bei der Methode des expliziten Klonens (EK) am 
meisten Rechenzeit, da hier die Systemkovarianzmatrix mit 38 x 38 am 
größten ist. Bei den Methoden des impliziten Klonens (IK und PIK) hat 


sie hier lediglich die einfache Größe von 19 x 19. Die Systemkovarianzma- 


182 


6.5 SC-EKF 


trizen der Offset-Methode (O) und der Methode des partiellen expliziten 
Klonens (PEK) haben eine Größe von 25 x 25 und die Rechenzeiten liegen 
dazwischen. Bei der Prädiktion benötigt die Methode partiellen explizi- 
ten Klonens (PEK) etwas weniger Rechenzeit als die Offset-Methode, da 
hier nur ein Teil der Systemkovarianzmatrix aktualisiert werden muss, vgl. 
Gleichung (3.22) bzw. Gleichung (5.19). Die größten Unterschiede ergeben 
sich bei den Aktualisierungen mit den 3D-LiDAR-Daten. Die Methode des 
impliziten Klonens mit Smoothing (IK) benötigt hier signifikant länger als 
die anderen Methoden, da das Smoothing mit ihrer Rückwärtsfilterung 
einen vielfachen Rechenaufwand bedeutet und Fk4m,k rekonstruiert wer- 
den muss. Das partielle implizite Klonen (PIK) benötigt demgegenüber nur 
geringfügig länger als die anderen Methoden, da hier nur mit einer 6x 6 
Systemkovarianzmatrix geglättet wird. Die Rechenzeit bei den expliziten 
Ansätzen ist demgegenüber sehr gering, da hier lediglich der Korrektur- 
schritt des Kalman-Filters anfällt. Die Rechenzeit der Offset-Methode (O) 
liegt dazwischen, da hier noch zusätzlich die Berechnung von Fk+m,k dazu- 
kommt. 

Zur Abschätzung des Rechenaufwands in der realen Anwendung kön- 
nen auch die kumulierten Rechenzeiten herangezogen werden, welche in 
Tabelle 6.9 aufgelistet sind. Hierfür werden die mittleren Rechenzeiten aller 
Sensoren mit ihrer jeweiligen Rate multipliziert und addiert, was dann die 
benötigte Rechenzeit pro Sekunde ergibt. Die kumulierten Rechenzeiten 
sind alle um die 10 ms, was einer Auslastung der verfügbaren Rechenzeit 
von 1% entspricht. Die Methoden des expliziten Klonens (EK) und dem Off- 
set (O) haben mit 12,6 ms bzw. 11,6 ms etwas höhere kumulierte Rechen- 
zeiten im Vergleich zu 7,9 ms und 8,9 ms für die Methoden mit partiellem 
explizitem Klonen (PEK) bzw. implizitem Klonen (IK). Die geringste kumu- 
lierte Rechenzeit weist das partielle implizite Klonen (PIK) mit 6,4 ms auf. 
D. h., die impliziten Methoden mit Smoothing weisen mit diesem Sensor- 
setup jeweils einen geringeren Rechenaufwand auf als die entsprechenden 
expliziten Methoden. Ein weiterer Vorteil der Methoden mit implizitem 
Klonen mit Smoothing (IK und PIK) ist, dass der zusätzliche Rechenauf- 
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wand für das stochastische Klonen entfällt, falls der relative Sensor aus- 
fällt oder temporär nicht genutzt werden soll. In der zweiten Zeile von 
Tabelle 6.9 sind die kumulierten Rechenzeiten ohne die Verarbeitung des 
LiDARs aufgelistet und hier haben die anderen Methoden alle einen höhe- 
ren Rechenaufwand als die Methoden mit implizitem Klonen (IK), da hier 
die augmentierten Matrizen bei den expliziten Methoden (EK und PEK) 


bzw. die zusätzlichen Offset-Variablen (O) trotzdem zu berechnen sind. 
Tabelle 6.9: Kumulierte Rechenzeiten pro s in ms. 


EK PEK IK PIK O 


mit LIDAR 12,6 79 89 64 11,6 
ohne LiDAR 12,3 76 56 5,4 10,5 


Ein weiterer Aspekt bei der Betrachtung der Rechenzeit ist die Skalier- 
barkeit, d. h. der Anstieg der Rechenzeit beim Hinzufügen weiterer relati- 
ver Sensoren. Hier ist bei den impliziten Methoden ein geringerer Anstieg 
zu erwarten, da die zusätzliche Rechenzeit für weitere Korrekturschritte 
der relativen Sensoren hinzuaddiert wird, wohingegen sich bei den anderen 
Methoden die Matrizen vergrößern. So ergibt sich durch einen weiteren 
relativen Sensor für das implizite Klonen (IK) eine kumulierte Rechenzeit 
von 12,2 ms bzw. von 7,4 ms für das partielle implizite Klonen (PIK). Damit 
sind beide immer noch geringer als die jeweilige explizite Methode mit nur 
einem Sensor (EK: 12,6 ms, PEK: 7,9 ms). 

Für die Methoden des impliziten Klonens (IK) und des expliziten Klo- 
nens (EK) wurde noch eine Evaluation der Skalierung der Rechenzeit auf 
einem weiteren Testsystem durchgeführt. Die Filter wurden mittels soge- 
nannten C++-Templates neu implementiert, sodass die Anzahl der zu inte- 
grierenden relativen Sensoren vor dem Kompilieren angepasst werden 
kann [Grö19]. Dies hat vor allem bei dem Filter mit explizitem Klonen den 
Vorteil, dass, falls kein relativer Sensor genutzt wird, die Berechnungen 
nicht mehr unnötig mit einem augmentierten Systemzustand durchgeführt 


werden müssen, wie es bei den Ergebnissen in Tabelle 6.9 ohne LiDAR 
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der Fall war. Allerdings bringt dies bei einem temporären Sensorausfall 
keine Vorteile hinsichtlich der Rechenzeit wie beim impliziten Klonen. Die 
kumulierten Rechenzeiten auf einem Intel Xeon E3-1245 mit vier Kernen 
und 3,3 GHz sind in Tabelle 6.10 aufgelistet. Bei diesem Vergleich wurde 
kein GNSS-Offset mitgeschätzt, d.h., die Größe der Systemkovarianzma- 
trix beträgt 16 x 16. Beim expliziten Klonen (EK) wird die augmentierte 
Systemkovarianzmatrix mit einem relativen Sensor (32x 32) bzw. mit zwei 
relativen Sensoren (48 x 48) entsprechend größer. Ohne zusätzlichen rela- 
tiven Sensor, also ohne stochastisches Klonen, zeigen beide Varianten die 
gleichen Rechenzeiten, da sich in dieser Implementierung auch ihre Matri- 
zengrößen und Korrekturschritte nicht unterscheiden. Bei dem impliziten 
Klonen (IK) steigt die kumulierte Rechenzeit mit den zusätzlichen relativen 
Sensoren jeweils um ca. 1,9 ms, da für jeden relativen Sensor ein Korrektur- 
schritt hinzukommt und beide Sensoren eine Rate von 10 Hz haben. Beim 
expliziten Klonen (EK) ist hingegen jeweils mehr als eine Verdoppelung 
der kumulierten Rechenzeit zu beobachten. Bei der Steigerung von einem 
auf zwei Sensoren nähert sich der Faktor von 2,5 dabei der theoretisch zu 
erwartenden Steigerung der Laufzeit bei Matrixmultiplikationen, welche 
zwischen 1,523797 = 2,6 und 1,5? = 3,4 liegt, vgl. Abschnitt 2.2.1. 


Tabelle 6.10: Kumulierte Rechenzeiten pro s in ms. 


IK EK 
ohne 4,1 4,1 
ein rel. Sensor @10Hz 5,9 11,6 


zwei rel. Sensoren @10Hz 7,8 29,1 


Die Methoden mit den inkrementellen Faktor-Graph-Algorithmen 
haben im Vergleich zu den Filter-Methoden signifikant héhere Rechenzei- 
ten. So hat der FGLM-Algorithmus eine mittlere Rechenzeit von 11,7 s mit 
einzelnen Spitzen bis zu 47,5s. Wie zu erwarten, benötigt der FGISAM- 
Algorithmus demgegenüber deutlich weniger Rechenzeit mit durchschnitt- 


lich 814,5 ms und Spitzen bis zu 21,1 s, was jedoch gegenüber den Filter- 
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Methoden immer noch sehr viel höher ist und einer Verwendung unter 
Echtzeitbedingungen entgegensteht. Vor dem erstem Schleifenschluss 
sind die benötigten Rechenzeiten deutlich geringer mit durchschnitt- 
lich 400,1ms und Spitzen bis zu 2,2s beim FGISAM-Algorithmus und 
durchschnittlich 1,8 s und Spitzen bis zu 6,2 s beim inkrementellen FGLM- 
Algorithmus. Hier wird deutlich, dass bei den Faktor-Graph-Algorithmen 
der Rechenaufwand mit der Zeit deutlich größer wird, wobei er wie zu 
erwarten bei dem FGISAM-Algorithmus weniger stark zunimmt und all- 
gemein auch deutlich niedriger liegt als beim FGLM-Algorithmus. 

Wie eingangs erwähnt, wurde das Scan-Matching bei der Evaluation 
für eine bessere Vergleichbarkeit vorberechnet. Es hat jedoch einen erhebli- 
chen Anteil an der Rechenzeit für die reale Anwendung. Die durchschnitt- 
liche Rechenzeit für ein Scan-Matching mittels GICP-Algorithmus liegt 
bei 265,8 ms, was deutlich über dem Messintervall des 3D-LiDARs von 
100 ms liegt. D. h., auf dem für die Evaluation verwendeten System könnte 
in der realen Anwendung nicht für jedes aufeinanderfolgende Scan-Paar 
das Scan-Matching berechnet werden. Damit auch während der Berech- 
nungsdauer eine Aktualisierung der Lokalisierung für andere Komponen- 
ten des autonomen mobilen Robotersystems mit hoher Rate erfolgen kann, 
ist eine parallele Filter-Struktur nach Abschnitt 5.3.4 notwendig. Die benö- 
tigte kumulierte Rechenzeit der Prädiktions- und Korrekturschritte der 
Filter ist hier in jedem Fall gering genug, damit das Filter, welches die 
3D-LiDAR-Daten verarbeitet, nach deren Verarbeitung stets zum Ausgabe- 
Filter mit der hohen Rate aufschließen kann. 

Wird eine inkrementelle Kartierung durchgeführt, sind zusätzlich noch 
die Rechenzeiten für die Aktualisierung der Karte zu berücksichtigen. Die 
in Abschnitt 5.3.2 vorgestellte inkrementelle Aktualisierung der NDT-Karte 
benötigt für die Integration eines Scans des 3D-LiDARs mit durchschnitt- 
lich ca. 130.000 Punkten im Mittel 42 ms mit der Aktualisierung der Exis- 
tenzwahrscheinlichkeiten mit Strahlverfolgung und nur um die 7 ms, wenn 
die Existenzwahrscheinlichkeiten unberücksichtigt bleiben. Damit liegt die 
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Rechenzeit der Aktualisierung der Karte in beiden Varianten deutlich unter 
dem Messintervall des 3D-LiDARs. 


6.5.5 Zeitstempelfilter 


Zur Evaluation des in Abschnitt 5.3.4 vorgestellten Zeitstempelfilters 
wurden Sensordaten mit und ohne Filterung der Zeitstempel aufgezeich- 
net. Abbildung 6.40 zeigt beispielhaft eine Sequenz der Zeitdifferenzen 
zwischen den Zeitstempeln aufeinanderfolgender Messungen der Xsens- 
IMU. Die spezifizierte Rate der IMU beträgt 100 Hz, d.h., die Zeitdifferenz 
sollte 10 ms betragen. Die Zeitdifferenzen der ungefilterten Daten weisen 
Schwankungen bis zu einigen Millisekunden auf, während bei den gefil- 
terten Daten in der Abbildung keine Schwankungen erkennbar sind. Die 
Mittelwerte liegen hingegen mit 10,0008 ms bzw. 10,0001 ms beide sehr 
dicht beieinander und nahe dem spezifizierten Wert. 

In Tabelle 6.11 sind die Standardabweichungen der Zeitstempeldiffe- 
renzen für die in den vorherigen Abschnitten verwendeten Sensoren im 
Vergleich zwischen ungefilterter und gefilterter Aufzeichnung aufgelistet. 
Die hier angegebene Datenrate des LiDARs bezieht sich auf die Rohdaten- 
pakete von denen mehrere zusammen einen Scan mit 10 Hz ergeben. Hier 
sind bei allen Sensoren Verbesserungen um mehrere Größenrodungen zu 
erkennen und es ergeben sich für die gefilterten Daten Standardabwei- 


chungen im einstelligen Mikrosekundenbereich. 


Tabelle 6.11: Standardabweichung o in us. 


ungefiltert gefiltert 


IMU @ 100Hz 589,2 1,6 
GNSS @ 4 Hz 2121,8 7,4 
Rad-Odometrie @ 10 Hz 1942,9 5,2 
LiDAR @ 347,22 Hz 26,8 0,3 
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Abbildung 6.40: Zeitstempeldifferenzen der IMU, welche eine Rate von 100 Hz hat. 


Bei dem in den vorherigen Abschnitten verwendeten Datensatz betrug 
die maximale gemessene Abweichung ohne Filterung weniger als Sms 
und mit einer maximalen Geschwindigkeit der mobilen Roboterplattform 
von 4m/s ergibt sich für eine Positionsschatzung ein potentieller Fehler 
von unter +2 cm. Die maximale von der IMU gemessene Drehrate betrug 
unter 40°/s was einen potentiellen Winkelfehler von weniger als +0,2° 
ergibt. D.h., der Einfluss der Zeitstempelabweichung auf die einzelnen 
Lokalisierungsschätzungen ist sehr gering. 

Die Filterung der Zeitstempel hat dennoch einen positiven Einfluss auf 
die Fusion für die Lokalisierung und Kartierung, da sich kleine Fehler in der 
Lage stärker auf die 3D-LiDAR-Messungen auswirken. So ergibt ein Win- 
kelfehler von 0,2° in einer Entfernung von 50 m eine Abweichung von ca. 
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(a) (b) 


Abbildung 6.41: Kartierungsergebnis ohne Zeitstempelfilter (a) und mit Zeitstempelfilter (b). 
Die relevanten Bereiche sind mit roten Ellipsen gekennzeichnet. Die Höhe ist farbkodiert von 
blau (niedrig) über grün zu rot (hoch). 


17 cm. Außerdem können sich bei SLAM auch kleine Abweichungen über 
die Zeit stärker auswirken. Wie in Abbildung 6.41 in einigen Bereichen, 
welche durch rote Ellipsen gekennzeichnet sind, zu erkennen ist, kann die 
Genauigkeit der Kartierung durch die Zeitstempelfilterung erhöht wer- 
den. Auf der linken Seite befindet sich ein lange gerade Hecke, welche 
in der Karte ohne die Zeitstempelfilterung eine Unterbrechung aufweist, 
wohingegen in der Karte mit Zeitstempelfilterung keine Unterbrechung 
der Hecke erkennbar ist. Auf der rechten Seite befindet sich ein Gebäude, 
welches in der Karte mit der Zeitstempelfilterung deutlich präziser kartiert 


wurde, erkennbar an der schmaleren Kontur. 
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6.5.6 Alternativer Datensatz 


Als alternativer Datensatz zur Verifikation der Vergleiche zwischen den 
Filter-Methoden mit stochastischem Klonen wurden Datensätze der KITTI 
Vision Benchmark Suite verwendet [Geil2; Gei13]. Die Datensätze sind 
unter http://www.cvlibs.net/datasets/kitti/ erhältlich. Die Daten- 
sätze sind im Rahmen der Forschung für autonomes Fahren im Straßenver- 
kehr aufgenommen worden und beinhalten somit urbane Szenarien und 
keine unstrukturierten Umgebungen. 

Die Ausgangsdaten enthalten Messungen eines INS/GNSS von OxTS, 
welches eine Lokalisierung mit hoher Rate von 100 Hz ausgibt. Hiervon 
wurden jedoch lediglich die Rohdaten verwendet, welche auch aus einem 
normalen GNSS und einer IMU kommen würden. Die zur Evaluation ver- 
wendeten GNSS-Daten umfassten den Längengrad, den Breitengrad, die 
Höhe und die 3D-Geschwindigkeit mit 10 Hz. Die IMU-Daten umfassten 
die 3D-Drehraten und die 3D-Beschleunigungen mit 100 Hz. Der verwen- 
dete Datensatz wies auch einige zeitweilige Ausfälle der Sensordaten auf. 
Da die Prädiktion im Filter mit den IMU-Daten durchgeführt wird, wurden 
die IMU-Daten für die Zeiten der Ausfälle linear interpoliert, die GNSS- 
Daten hingegen wurden mit Lücken verwendet. Als Umgebungssensor 
kommt ebenfalls ein Velodyne HDL-64 zum Einsatz und die Datensätze 
enthalten bereits inertial korrigierte 3D-Scans. 

Es wurde zunächst der Datensatz „Sequenz 00“ ausgewählt, da für die- 
sen eine Referenzlokalisierung erhältlich ist und er eine lange Strecke von 
über 3,7 km abdeckt. Die 3D-Positionsfehler der Methoden mit stochasti- 
schem Klonen bezogen auf die Referenzlokalisierung sind in Tabelle 6.12 
aufgeführt. 

Die mittleren Fehler @3p sind mit 14 cm alle sehr gering und unterschei- 
den sich nur um ein bis zwei Millimeter. Bei den maximalen Fehlern hat 
das Filter mit dem Offset einen um lediglich 3 cm höheren Fehler. Dass 
alle Ansätze so nahe beisammen liegen, hängt mit dem GNSS zusammen, 
welches sehr geringe Unsicherheiten aufweist, und somit der Einfluss der 
anderen Messungen auf die Lokalisierungsschätzung geringer ausfällt. Aus 
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Tabelle 6.12: 3D-Positionsfehler in m. 


Zon max(é3p) 


EK 0,14 1,90 
PEK 0,14 1,90 
IK 0,14 1,90 
PIK 0,14 1,90 


O 0,14 1,93 


diesem Grund sind auch die Einfliisse der Integrationsmethode der rela- 
tiven Messungen geringer. Abbildung 6.42 zeigt das Kartierungsergebnis 
mit der Filter-Methode des impliziten Klonens mit Smoothing, welches mit 
einer Straßenkarte hinterlegt wurde. 

Da sich mit GNSS kaum Unterschiede zwischen den Methoden ergaben, 
wurde auch hier ein Ausfall des GNSS simuliert und die 3D-Positionsfehler 
gegenüber der Referenzlokalisierung evaluiert, siehe Abbildung 6.43. Der 
Ausfall beginnt nach 8,5 s, damit die Filter Zeit hatten einzuschwingen, und 
bleibt für die restliche Zeit bestehen, d.h., es stehen ab diesem Zeitpunkt 
nur noch die IMU-Daten und das Scan-Matching der 3D-LiDAR-Daten 
als relative Posenmessung für die Lokalisierungsschätzung zur Verfügung. 
In der Abbildung ist der Beginn des Ausfalls mit einer vertikalen gestri- 
chelten Linie gekennzeichnet. Bei allen Methoden ist der Fehler mit der 
Zeit tendenziell steigend. Das partielle explizite Klonen (PEK) ist wieder 
nicht in die Abbildung aufgenommen, da es weniger als 7 cm Unterschied 
zum kompletten expliziten Klonen aufwies. Der Unterschied zwischen dem 
impliziten Klonen mit Smoothing (IK) und dessen partieller Methode (PIK) 
fällt hier mit zunehmender Zeit größer aus und auch der Unterschied zu der 
expliziten Methode (EK) wird im späteren Verlauf deutlicher als bei dem 
kurzen Ausfall in Abschnitt 6.5.2. Wie schon bei den Ergebnissen zuvor, 
unterscheidet sich der Verlauf bei der Offset-Methode (O) schon zu Beginn 
deutlichen von den anderen Methoden. 
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Abbildung 6.42: Ergebnis der Kartierung mit der Filter-Methode des impliziten Klonens mit 
Smoothing über einer Straßenkarte. Die Höhe ist farbkodiert von blau (niedrig) über grün 
zu rot (hoch). Die Straßenkarte im Hintergrund ist von OpenstreetMap: ©OpenStreetMap- 
Mitwirkende (https ://www.openstreetmap.org/copyright). 
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Abbildung 6.43: 3D-Positionsfehler mit GNSS-Ausfall. Der Beginn des Ausfalls ist mit einer 
vertikalen gestrichelten Linie gekennzeichnet. 


Stehen nur relative Positionsmessungen für lange Zeiträume zur Ver- 
fügung, bietet sich zur Evaluation die Betrachtung eines relativen Fehlers 
bezogen auf die zurückgelegte Wegstrecke an. Dieser relative Fehler wird 
wie folgt definiert: 


Q3D,k = esp,k/Sk, (6.7) 


wobei sx die bis dahin zurückgelegte Strecke ist. 

Abbildung 6.44 zeigt den zeitlichen Verlauf des relativen 3D-Positi- 
onsfehlers. Dieser ist zu Beginn für alle Methoden sehr hoch, da hier die 
zurückgelegte Wegstrecke sehr klein ist und sich damit kleine Abweichun- 
gen sehr stark auswirken. Mit der Zeit fallen die relativen 3D-Positionsfeh- 
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ler aller Methoden auf unter 1% ab und weisen nur noch geringe Schwan- 


kungen auf. 


Tabelle 6.13: Mittlerer relativer 3D-Positionsfehler in %. 


Aan Aan ab 150s 


EK 1,09 0,59 
PEK 1,09 0,59 
IK 0,77 0,62 
PIK 0,78 0,64 


O 1,69 0,72 


In Tabelle 6.13 sind die mittleren relativen 3D-Positionsfehler für die 
verschiedenen Methoden aufgelistet. Wobei dieser jeweils einmal für die 
gesamte Dauer und einmal für den Zeitraum ab 150 s ermittelt wurde. Letz- 
teres wurde mit aufgenommen, da aus Abbildung 6.44 ersichtlich ist, dass 
die relativen 3D-Positionsfehler der verschiedenen Methoden im späteren 
Verlauf recht nahe beieinanderliegen und die anfänglichen großen Abwei- 
chungen keine Rolle mehr spielen. Die Methoden des impliziten Klonens 
mit Smoothing (IK und PIK) zeigen hier über den gesamten Zeitraum die 
geringsten relativen 3D-Positionsfehler im zweiten Zeitraum sind jedoch 
die Methoden des expliziten Klonens (EK und PEK) besser. Die Offset- 
Methode (O) zeigt vor allem in dem gesamten Zeitraum einen viel höheren 
mittleren relativen 3D-Positionsfehler, da hier der relative 3D-Positionsfeh- 
ler am Anfang sehr viel länger als die anderen Methoden braucht, um unter 
1% zu fallen. In dem späteren Zeitraum liegt der mittlere relative 3D-Posi- 
tionsfehler der Methode mit dem Offset näher an den anderen Methoden, 
welche sehr ähnliche Ergebnisse liefern. 

Zur Verifikation wurden noch zwei weitere Datensätze, „Sequenz 02“ 
und „Sequenz 05“, hinzugezogen. Zusammen sind mit den drei Datensät- 
zen insgesamt knapp über 10,9 km Fahrstrecke abgedeckt. In Tabelle 6.14 
sind die zusammengefassten mittleren 3D-Positionsfehler über alle drei 


Datensätze aufgelistet und es zeigen sich die gleichen Verhältnisse wie 
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Abbildung 6.44: Relativer 3D-Positionsfehler mit GNSS-Ausfall. Der Beginn des Ausfalls ist 
mit einer vertikalen gestrichelten Linie gekennzeichnet. 


bei der Analyse nur des Datensatzes „Sequenz 00“. So sind die impliziten 
Methoden (IK und PIK) über die gesamte Zeit leicht besser als die explizi- 
ten Methoden (EK und PEK) und umgekehrt für den Zeitraum ab 150 s. Die 
Methode mit dem Offset (O) weist wieder jeweils einen höheren mittleren 
relativen 3D-Positionsfehler auf. 


Dynamische Objekte 


Der Datensatz „Sequenz 00“ enthält auch dynamische Objekte und kann 
daher für die Analyse der Eignung der Existenzwahrscheinlichkeit zu deren 
Behandlung bei der Kartierung nach Abschnitt 5.3.2 herangezogen werden. 
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Tabelle 6.14: Mittlerer relativer 3D-Positionsfehler in %. 


d3D dan ab 150s 


EK 1,47 0,99 
PEK 1,47 0,99 
IK 1,39 1,10 
PIK 1,41 1,12 
O 1,82 1,30 


(a) (b) 


Abbildung 6.45: Kartierung in einer Situation mit dynamischen Objekten. Einfache Kartie- 
rung (a), mit Existenzwahrscheinlichkeiten (b). Die Höhe ist farbkodiert von blau (niedrig) 
über grün zu rot (hoch). 


Abbildung 6.45 zeigt einen Vergleich zwischen der einfachen Kartierung 
und der Kartierung mit der Einbeziehung der Existenzwahrscheinlichkeit. 
In Abbildung 6.45a mit der einfachen Kartierung sind deutliche, langge- 
zogene Artefakte von vorbeifahrenden Objekten zu erkennen. In Abbil- 
dung 6.45b sind diese nicht mehr vorhanden, d. h., sie konnten erfolgreich 
durch die Schätzung einer Existenzwahrscheinlichkeit pro Voxel eliminiert 


werden. 
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6.5.7 Zusammenfassung 


Bei der Evaluation mit dem simulierten GNSS-Ausfall haben die Metho- 
den des impliziten Klonens mit Smoothing und mit dem expliziten Klonen 
nahezu gleiche Ergebnisse erzielt, welche deutlich besser sind als bei der 
Methode mit zusätzlichem Offset. Mit dem Zurückpropagieren der Informa- 
tion durch das Smoothing bei dem impliziten Klonen kann in etwa die glei- 
che Genauigkeit wie beim expliziten Klonen erreicht werden. Während bei 
dem partiellen expliziten Klonen nahezu kein Unterschied zum kompletten 
expliziten Klonen zu beobachten war, sind die Unterschiede zwischen dem 
partiellen und kompletten impliziten Klonen mit Smoothing deutlicher, 
da die Rekonstruktion der augmentierten Systemkovarianzmatrix beim 
partiellen impliziten Klonen eine zusätzliche Approximation erfordert. 

Beim Vergleich zu einer Referenzlokalisierung von einem Faktor- 
Graph-Algorithmus haben alle Filter ähnliche Ergebnisse gezeigt, bis auf 
die Methode mit dem zusätzlichen Offset, welche beim Gierwinkel größere 
Abweichungen aufwies als die anderen Methoden. Die Einbeziehung eines 
GNSS-Offsets für das Schließen von Schleifen hat Vorteile gezeigt, wenn 
der relative 3D-Positionsfehler betrachtet wird und es konnten damit qua- 
litativ bessere Karten erzeugt werden. Die inkrementellen Faktor-Graph- 
Methoden haben gegenüber den Filter-Methoden in der Genauigkeit nur 
leichte Vorteile gezeigt, was auch an einer leicht besseren Karte zu erken- 
nen war. Es konnte jedoch mit keiner der schritthaltenden Methoden die 
Genauigkeit der Referenzlokalisierung erreicht werden, weshalb für eine 
möglichst genaue Kartenerstellung eine abschließende Optimierung über 
den gesamten Pfad vorzuziehen ist. Dies ist jedoch nur möglich, wenn die 
Karte erst anschließend benötigt wird. 

Bei den Betrachtungen zum Rechenaufwand benötigten die Filter- 
Methoden im Vergleich zu den inkrementellen Faktor-Graph-Methoden 
sehr viel weniger Rechenzeit. Alle Filter-Methoden sind mit einer Auslas- 
tung der verfügbaren Rechenzeit von ca. 1 % sehr gut für reale Anwendun- 
gen und in Kombination mit einer Parallelverarbeitung für eine konstant 
hohe Ausgaberate geeignet. Dabei benötigen die Filter-Methoden des 
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impliziten Klonens mit dem Smoothing jeweils weniger Rechenzeit als die 
expliziten Methoden. 

Da das partielle explizite Klonen in der Genauigkeit kaum einen Unter- 
schied zum kompletten expliziten Klonen aufweist, ist es diesem aufgrund 
des geringeren Rechenaufwands vorzuziehen. Sollen bestehende Filter um 
die Funktionalität des stochastischen Klonens erweitert werden, empfiehlt 
sich das implizite Klonen mit Smoothing, da die bestehenden Filterele- 
mente, wie Zustandsvektor und Systemkovarianzmatrix, nicht angepasst 
werden müssen. Außerdem weist es gegenüber dem expliziten Klonen 
einen geringeren Rechenaufwand auf. Darüber hinaus fallen die Berech- 
nungen für das stochastische Klonen nur an, wenn auch Messungen der 
relativen Sensoren verarbeitet werden. D.h., auch bei sich ändernden Sen- 
sorkonfigurationen kann das implizite Klonen mit Smoothing flexibler ein- 
gesetzt werden. Sollen weitere relative Sensoren integriert werden, wei- 
sen die impliziten Methoden eine bessere Skalierbarkeit hinsichtlich des 
Rechenaufwands auf. 

Mit der Anwendung der Zeitstempelfilterung wurde eine deutliche 
Verringerung der Schwankungen der Zeitstempeldifferenzen aufeinander- 
folgender Messdaten erzielt und bei der resultierenden Kartierung konnte 
eine verbesserte Genauigkeit beobachtet werden. 

Zur Bestätigung der Resultate konnten bei der Evaluation anhand von 
alternativen Datensätzen mit den Filter-Methoden des impliziten Klonens 
wieder vergleichbare Ergebnisse zu den Methoden des expliziten Klonens 
erzielt werden. 
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Kapitel 7 


Zusammenfassung und 
Ausblick 


Nach einer Zusammenfassung der vorgestellten Arbeiten soll ein Ausblick 
mögliche folgende und darauf aufbauende Weiterentwicklungen beleuch- 


ten. 


7.1 Zusammenfassung 


Das Ziel der vorliegenden Arbeit war es, Methoden für die Kombination 
und Integration mehrerer Sensoren für die Lokalisierung und Kartenerstel- 
lung in heterogenen Umgebungen zu entwickeln. 

Es wurde ein erweitertes Landmarkenmodell mit zusätzlichen ortsun- 
abhängigen und visuellen Merkmalen für die robustere Wiedererkennung 
entwickelt und in einen RBPF-SLAM-Algorithmus integriert. Mit der ver- 
besserten Datenzuordnung konnte gezeigt werden, dass weniger Partikel 
für das erfolgreiche Schließen der Schleifen benötigt werden und somit 
das Erstellen einer konsistenten Karte vereinfacht wird. Darüber hinaus 
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konnte gezeigt werden, dass mit dem erweiterten Landmarkenmodell auch 
die Präzision der Karte erhöht wird. 

Durch die konsequente Auslegung der Erweiterungen des RBPF-SLAM- 
Algorithmus in probabilistisch konsistenter Weise war es möglich, die 
Kombination einer Landmarkenkarte mit einer 2D-Rasterkarte für die 
Kartierung von Mischgebieten in einem hybriden Ansatz zu realisieren. 
Hierdurch konnten genauere Karten im Vergleich zu den Einzelansätzen 
erzeugt werden. 

Für die Kombination von SLAM mit Multi-Sensor-Fusion wurde eben- 
falls die modulare Erweiterbarkeit des RBPF-SLAM-Algorithmus genutzt, 
um einen zusätzlichen absoluten Positionssensor für die Begrenzung der 
globalen Unsicherheit zu integrieren. Durch die Fusion mit dem absoluten 
Sensor konnte die Robustheit hinsichtlich des Schließens großer Schlei- 
fen effektiv gesteigert werden. So konnte gezeigt werden, dass bei großen 
Schleifen weniger Partikel zum Erstellen genauer Karten notwendig sind 
bzw. bei gleicher Partikelanzahl die Zielverteilung besser approximiert 
wird und damit auch die benötigte Rechenzeit verringert wird. 

Für die Integration eines zusätzlichen relativen Sensors in ein Lokalisie- 
rungsfilter mittels stochastischem Klonen wurde eine neuartige Methode 
mit implizitem Klonen in Kombination mit Smoothing vorgestellt und 
dem expliziten Klonen gegenübergestellt. Die Methoden des stochasti- 
schen Klonens wurden miteinander verglichen und es konnten bei der 
Evaluation mit einem simulierten GNSS-Ausfall vergleichbare Ergebnisse 
erzielt werden. Beim Vergleich der Methoden zu einer Referenzlokalisie- 
rung von einem Faktor-Graph-Algorithmus konnten sie ebenfalls ähnliche 
Ergebnisse erzielen. Mit dem für das Schließen von Schleifen eingeführten 
GNSS-Offset konnten ein geringerer relativer 3D-Positionsfehler erzielt 
und qualitativ bessere Karten erzeugt werden. Die ebenfalls zum Vergleich 
herangezogenen inkrementellen Faktor-Graph-Methoden zeigten gegen- 
über den Filter-Methoden in der Genauigkeit nur leichte Vorteile. Bei der 
Evaluation der Rechenzeit hingegen zeigten sich die Filter-Methoden im 


Vergleich zu den inkrementellen Faktor-Graph-Methoden weit überlegen 
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7.2 Ausblick 


und für reale Echtzeit-Anwendungen sehr gut geeignet. In Verbindung mit 
der Parallelverarbeitung können mit den Filtermethoden trotz der Integra- 
tion des rechenaufwändigen Scan-Matchings konstant hohe Ausgaberaten 
erreicht werden. 

Mit der Anwendung der Zeitstempelfilterung konnte eine deutliche 
Verringerung der Schwankungen der Zeitstempeldifferenzen aufeinander- 
folgender Messdaten erzielt werden und auch bei der resultierenden Kar- 
tierung konnte eine verbesserte Genauigkeit beobachtet werden. 

Ein weiterer Aspekt der Arbeit war eine möglichst praxisnahe Umset- 
zung der entwickelten Methoden und deren Einsatz in realen Anwendungs- 
gebieten. So konnten mit der Implementierung der vorgestellten Algorith- 
men sowie der Umsetzung der Zeitstempelfilterung und der parallelen 
Verarbeitung umfassende Verbesserungen bei der Lokalisierung und Kar- 
tierung für mobile Robotersysteme geschaffen werden. Diese reichen von 
der Akquise der Sensordaten über die Fusion mehrerer relativer und abso- 
luter Sensoren bis hin zur Integration in SLAM-Algorithmen. Ein Großteil 
der vorgestellten Algorithmen ist auf verschiedenen Forschungsplattfor- 
men mit unterschiedlichen Sensorausstattungen erfolgreich im Einsatz. 
Dazu gehören verschiedene Landfahrzeuge und schwere Arbeitsmaschi- 
nen sowie Wasserfahrzeuge [Heil9; Emt17; Kle20]. 


7.2 Ausblick 


Das Scan-Matching von Punktwolken mit einer großen Anzahl von Punk- 
ten ist sehr rechenaufwändig und konnte auf dem zur Evaluation verwen- 
deten System nicht mit der vollen Rate des 3D-LiDARs ausgeführt werden. 
Hier besteht das Potential für Verbesserungen in Form alternativer Algo- 
rithmen, welche das Scan-Matching effizienter ausführen können, oder die 
Portierung des GICP-Algorithmus auf massiv parallele Plattformen wie 
beispielsweise Grafikprozessoren (englisch: general-purpose computation 


on graphics processing units - GPGPU). 
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7 Zusammenfassung und Ausblick 


Als Alternative zu der Verwendung von einem 3D-LiDAR mit Scan- 
Matching könnte auch eine kamera-basierte visuelle Odometrie eingesetzt 
werden, wobei bei monokularer visueller Odometrie zusätzlich die Skalie- 
rung geschätzt werden muss, da diese mit nur einer Kamera nicht beob- 
achtbar ist. 

Da die Genauigkeit der Höhenmessung des GNSS im Vergleich zu den 
horizontalen Messungen sehr viel schlechter ist, wäre die Fusion mit einem 
weiteren, die Höhe messenden Sensor, wie zum Beispiel einem barometri- 
schen Drucksensor, eine zu untersuchende Option [Zal14]. 

Die inkrementellen Faktor-Graph-Methoden haben gegenüber den Fil- 
tern mit stochastischem Klonen für die Lokalisierung nur eine kleine Ver- 
besserung bei der Genauigkeit gezeigt und dabei wesentlich mehr Rechen- 
zeit benötigt. Sie haben jedoch den Vorteil, dass bei der Optimierung auch 
alle zurückliegenden Posen verbessert werden, was i. A. im späteren Ver- 
lauf in einer präziseren Karte resultiert. Daher könnte ein hybrides Verfah- 
ren mit einem SC-EKF für die Echtzeit-Lokalisierung und lokale Kartierung 
in Kombination mit einem inkrementellen Faktor-Graph-Algorithmus für 


die Kartierung einer globalen Karte die Vorteile beider Methoden vereinen. 
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